/*
* Copyright (c) 2013 Google, Inc.
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "box2d/b2_particle_system.h"
#include "box2d/b2_particle_group.h"
#include "box2d/b2_block_allocator.h"
#include "box2d/b2_world.h"
#include "box2d/b2_world_callbacks.h"
#include "box2d/b2_body.h"
#include "box2d/b2_fixture.h"
#include "box2d/b2_shape.h"
#include "box2d/b2_edge_shape.h"
#include <algorithm>

#include "b2_voronoi_diagram.h"

#ifdef ENABLE_LIQUID

static const uint32 xTruncBits = 12;
static const uint32 yTruncBits = 12;
static const uint32 tagBits = 8u * sizeof(uint32);
static const uint32 yOffset = 1u << (yTruncBits - 1u);
static const uint32 yShift = tagBits - yTruncBits;
static const uint32 xShift = tagBits - yTruncBits - xTruncBits;
static const uint32 xScale = 1u << xShift;
static const uint32 xOffset = xScale * (1u << (xTruncBits - 1u));
static const uint32 yMask = ((1u << yTruncBits) - 1u) << yShift;
static const uint32 xMask = ~yMask;
static const uint32 relativeTagRight = 1u << xShift;
static const uint32 relativeTagBottomLeft = (uint32)((1 << yShift) +
                                                    ((~uint32(0)) << xShift));

static const uint32 relativeTagBottomRight = (1u << yShift) + (1u << xShift);

// This functor is passed to std::remove_if in RemoveSpuriousBodyContacts
// to implement the algorithm described there.  It was hoisted out and friended
// as it would not compile with g++ 4.6.3 as a local class.  It is only used in
// that function.
class b2ParticleBodyContactRemovePredicate
{
public:
  b2ParticleBodyContactRemovePredicate(b2ParticleSystem* system,
                     int32* discarded)
    : m_system(system), m_lastIndex(-1), m_currentContacts(0),
      m_discarded(discarded) {}

  bool operator()(const b2ParticleBodyContact& contact)
  {
    // This implements the selection criteria described in
    // RemoveSpuriousBodyContacts().
    // This functor is iterating through a list of Body contacts per
    // Particle, ordered from near to far.  For up to the maximum number of
    // contacts we allow per point per step, we verify that the contact
    // normal of the Body that genenerated the contact makes physical sense
    // by projecting a point back along that normal and seeing if it
    // intersects the fixture generating the contact.

    if (contact.index != m_lastIndex)
    {
      m_currentContacts = 0;
      m_lastIndex = contact.index;
    }

    if (m_currentContacts++ > k_maxContactsPerPoint)
    {
      ++(*m_discarded);
      return true;
    }

    // Project along inverse normal (as returned in the contact) to get the
    // point to check.
    b2Vec2 n = contact.normal;
    // weight is 1-(inv(diameter) * distance)
    n *= m_system->m_particleDiameter * (1 - contact.weight);
    b2Vec2 pos = m_system->m_positionBuffer.data[contact.index] + n;

    // pos is now a point projected back along the contact normal to the
    // contact distance. If the surface makes sense for a contact, pos will
    // now lie on or in the fixture generating
    if (!contact.fixture->TestPoint(pos))
    {
      float32 distance;
      b2Vec2 normal;
      contact.fixture->ComputeDistance(pos, &distance, &normal);
      if (distance < b2_linearSlop)
      {
        return false;
      }
      ++(*m_discarded);

      return true;
    }

    return false;
  }
private:
  // Max number of contacts processed per particle, from nearest to farthest.
  // This must be at least 2 for correctness with concave shapes; 3 was
  // experimentally arrived at as looking reasonable.
  static const int32 k_maxContactsPerPoint = 3;
  const b2ParticleSystem* m_system;
  // Index of last particle processed.
  int32 m_lastIndex;
  // Number of contacts processed for the current particle.
  int32 m_currentContacts;
  // Output the number of discarded contacts.
  int32* m_discarded;
};

namespace {

// Compares the expiration time of two particle indices.
class ExpirationTimeComparator
{
public:
  // Initialize the class with a pointer to an array of particle
  // lifetimes.
  ExpirationTimeComparator(const int32* const expirationTimes) :
    m_expirationTimes(expirationTimes)
  {
  }
  // Empty destructor.
  ~ExpirationTimeComparator() { }

  // Compare the lifetime of particleIndexA and particleIndexB
  // returning true if the lifetime of A is greater than B for particles
  // that will expire.  If either particle's lifetime is infinite (<= 0.0f)
  // this function return true if the lifetime of A is lesser than B.
  // When used with std::sort() this results in an array of particle
  // indicies sorted in reverse order by particle lifetime.
  // For example, the set of lifetimes
  // (1.0, 0.7, 0.3, 0.0, -1.0, -2.0)
  // would be sorted as
  // (0.0, -1.0, -2.0, 1.0, 0.7, 0.3)
  bool operator() (const int32 particleIndexA,
           const int32 particleIndexB) const
  {
    const int32 expirationTimeA = m_expirationTimes[particleIndexA];
    const int32 expirationTimeB = m_expirationTimes[particleIndexB];
    const bool infiniteExpirationTimeA = expirationTimeA <= 0.0f;
    const bool infiniteExpirationTimeB = expirationTimeB <= 0.0f;
    return infiniteExpirationTimeA == infiniteExpirationTimeB ?
      expirationTimeA > expirationTimeB : infiniteExpirationTimeA;
  }

private:
  const int32* m_expirationTimes;
};

// *Very* lightweight pair implementation.
template<typename A, typename B>
struct LightweightPair
{
  A first;
  B second;

  // Compares the value of two FixtureParticle objects returning
  // true if left is a smaller value than right.
  static bool Compare(const LightweightPair& left,
            const LightweightPair& right)
  {
    return left.first < right.first &&
      left.second < right.second;
  }

};

// Allocator for a fixed set of items.
class FixedSetAllocator
{
public:
  // Associate a memory allocator with this object.
  FixedSetAllocator(b2StackAllocator* allocator);
  // Deallocate storage for this class.
  ~FixedSetAllocator()
  {
    Clear();
  }

  // Allocate internal storage for this object returning the size.
  int32 Allocate(const int32 itemSize, const int32 count);

  // Deallocate the internal buffer if it's allocated.
  void Clear();

  // Get the number of items in the set.
  int32 GetCount() const { return m_count; }

  // Invalidate an item from the set by index.
  void Invalidate(const int32 itemIndex)
  {
    b2Assert(m_valid);
    m_valid[itemIndex] = 0;
  }

  // Get the buffer which indicates whether items are valid in the set.
  const int8* GetValidBuffer() const { return m_valid; }

protected:
  // Get the internal buffer.
  void* GetBuffer() const { return m_buffer; }
  void* GetBuffer() { return m_buffer; }

  // Reduce the number of items in the set.
  void SetCount(int32 count)
  {
    b2Assert(count <= m_count);
    m_count = count;
  }

private:
  // Set buffer.
  void* m_buffer;
  // Array of size m_count which indicates whether an item is in the
  // corresponding index of m_set (1) or the item is invalid (0).
  int8* m_valid;
  // Number of items in m_set.
  int32 m_count;
  // Allocator used to allocate / free the set.
  b2StackAllocator* m_allocator;
};

// Allocator for a fixed set of objects.
template<typename T>
class TypedFixedSetAllocator : public FixedSetAllocator
{
public:
  // Initialize members of this class.
  TypedFixedSetAllocator(b2StackAllocator* allocator) :
    FixedSetAllocator(allocator) { }

  // Allocate a set of objects, returning the new size of the set.
  int32 Allocate(const int32 numberOfObjects)
  {
    Clear();
    return FixedSetAllocator::Allocate(sizeof(T), numberOfObjects);
  }

  // Get the index of an item in the set if it's valid return an index
  // >= 0, -1 otherwise.
  int32 GetIndex(const T* item) const
  {
    if (item)
    {
      b2Assert(item >= GetBuffer() &&
           item < GetBuffer() + GetCount());
      const int32 index =
        (int32)(((uint8*)item - (uint8*)GetBuffer()) /
            sizeof(*item));
      if (GetValidBuffer()[index])
      {
        return index;
      }
    }
    return -1;
  }

  // Get the internal buffer.
  const T* GetBuffer() const
  {
    return (const T*)FixedSetAllocator::GetBuffer();
  }
  T* GetBuffer() { return (T*)FixedSetAllocator::GetBuffer(); }
};

// Associates a fixture with a particle index.
typedef LightweightPair<b2Fixture*,int32> FixtureParticle;

// Associates a fixture with a particle index.
typedef LightweightPair<int32,int32> ParticlePair;

}  // namespace

// Set of fixture / particle indices.
class FixtureParticleSet :
  public TypedFixedSetAllocator<FixtureParticle>
{
public:
  // Initialize members of this class.
  FixtureParticleSet(b2StackAllocator* allocator) :
    TypedFixedSetAllocator<FixtureParticle>(allocator) { }


  // Initialize from a set of particle / body contacts for particles
  // that have the b2_fixtureContactListenerParticle flag set.
  void Initialize(const b2ParticleBodyContact * const bodyContacts,
          const int32 numBodyContacts,
          const uint32 * const particleFlagsBuffer);

  // Find the index of a particle / fixture pair in the set or -1
  // if it's not present.
  // NOTE: This was not written as a template function to avoid
  // exposing any dependencies via this header.
  int32 Find(const FixtureParticle& fixtureParticle) const;
};

// Set of particle / particle pairs.
class b2ParticlePairSet : public TypedFixedSetAllocator<ParticlePair>
{
public:
  // Initialize members of this class.
  b2ParticlePairSet(b2StackAllocator* allocator) :
    TypedFixedSetAllocator<ParticlePair>(allocator) { }

  // Initialize from a set of particle contacts.
  void Initialize(const b2ParticleContact * const contacts,
          const int32 numContacts,
          const uint32 * const particleFlagsBuffer);

  // Find the index of a particle pair in the set or -1
  // if it's not present.
  // NOTE: This was not written as a template function to avoid
  // exposing any dependencies via this header.
  int32 Find(const ParticlePair& pair) const;
};

static inline uint32 computeTag(float32 x, float32 y)
{
  return ((uint32)(y + yOffset) << yShift) + (uint32)(xScale * x + xOffset);
}

static inline uint32 computeRelativeTag(uint32 tag, int32 x, int32 y)
{
  return tag + (y << yShift) + (x << xShift);
}

b2ParticleSystem::InsideBoundsEnumerator::InsideBoundsEnumerator(
  uint32 lower, uint32 upper, const Proxy* first, const Proxy* last)
{
  m_xLower = lower & xMask;
  m_xUpper = upper & xMask;
  m_yLower = lower & yMask;
  m_yUpper = upper & yMask;
  m_first = first;
  m_last = last;
  b2Assert(m_first <= m_last);
}

int32 b2ParticleSystem::InsideBoundsEnumerator::GetNext()
{
  while (m_first < m_last)
  {
    uint32 xTag = m_first->tag & xMask;
#if B2_ASSERT_ENABLED
    uint32 yTag = m_first->tag & yMask;
    b2Assert(yTag >= m_yLower);
    b2Assert(yTag <= m_yUpper);
#endif
    if (xTag >= m_xLower && xTag <= m_xUpper)
    {
      return (m_first++)->index;
    }
    m_first++;
  }
  return b2_invalidParticleIndex;
}

b2ParticleSystem::b2ParticleSystem(const b2ParticleSystemDef* def,
                   b2World* world) :
  m_handleAllocator(b2_minParticleSystemBufferCapacity),
  m_stuckParticleBuffer(world->m_blockAllocator),
  m_proxyBuffer(world->m_blockAllocator),
  m_contactBuffer(world->m_blockAllocator),
  m_bodyContactBuffer(world->m_blockAllocator),
  m_pairBuffer(world->m_blockAllocator),
  m_triadBuffer(world->m_blockAllocator)
{
  b2Assert(def);
  m_paused = false;
  m_timestamp = 0;
  m_allParticleFlags = 0;
  m_needsUpdateAllParticleFlags = false;
  m_allGroupFlags = 0;
  m_needsUpdateAllGroupFlags = false;
  m_hasForce = false;
  m_iterationIndex = 0;

  m_count = 0;
  m_internalAllocatedCapacity = 0;
  m_forceBuffer = NULL;
  m_weightBuffer = NULL;
  m_staticPressureBuffer = NULL;
  m_accumulationBuffer = NULL;
  m_accumulation2Buffer = NULL;
  m_depthBuffer = NULL;
  m_groupBuffer = NULL;

  SetStrictContactCheck(def->strictContactCheck);
  SetDensity(def->density);
  SetGravityScale(def->gravityScale);
  SetRadius(def->radius);
  SetMaxParticleCount(def->maxCount);

  m_groupCount = 0;
  m_groupList = NULL;

  b2Assert(def->lifetimeGranularity > 0.0f);
  m_def = *def;

  m_world = world;

  m_stuckThreshold = 0;

  m_timeElapsed = 0;
  m_expirationTimeBufferRequiresSorting = false;

  SetDestructionByAge(m_def.destroyByAge);
}

b2ParticleSystem::~b2ParticleSystem()
{
  while (m_groupList)
  {
    DestroyParticleGroup(m_groupList);
  }

  FreeUserOverridableBuffer(&m_handleIndexBuffer);
  FreeUserOverridableBuffer(&m_flagsBuffer);
  FreeUserOverridableBuffer(&m_lastBodyContactStepBuffer);
  FreeUserOverridableBuffer(&m_bodyContactCountBuffer);
  FreeUserOverridableBuffer(&m_consecutiveContactStepsBuffer);
  FreeUserOverridableBuffer(&m_positionBuffer);
  FreeUserOverridableBuffer(&m_velocityBuffer);
  FreeUserOverridableBuffer(&m_colorBuffer);
  FreeUserOverridableBuffer(&m_userDataBuffer);
  FreeUserOverridableBuffer(&m_expirationTimeBuffer);
  FreeUserOverridableBuffer(&m_indexByExpirationTimeBuffer);
  FreeBuffer(&m_forceBuffer, m_internalAllocatedCapacity);
  FreeBuffer(&m_weightBuffer, m_internalAllocatedCapacity);
  FreeBuffer(&m_staticPressureBuffer, m_internalAllocatedCapacity);
  FreeBuffer(&m_accumulationBuffer, m_internalAllocatedCapacity);
  FreeBuffer(&m_accumulation2Buffer, m_internalAllocatedCapacity);
  FreeBuffer(&m_depthBuffer, m_internalAllocatedCapacity);
  FreeBuffer(&m_groupBuffer, m_internalAllocatedCapacity);
}

template <typename T> void b2ParticleSystem::FreeBuffer(T** b, int capacity)
{
  if (*b == NULL)
    return;

  m_world->m_blockAllocator.Free(*b, sizeof(**b) * capacity);
  *b = NULL;
}

// Free buffer, if it was allocated with b2World's block allocator
template <typename T> void b2ParticleSystem::FreeUserOverridableBuffer(
  UserOverridableBuffer<T>* b)
{
  if (b->userSuppliedCapacity == 0)
  {
    FreeBuffer(&b->data, m_internalAllocatedCapacity);
  }
}

// Reallocate a buffer
template <typename T> T* b2ParticleSystem::ReallocateBuffer(
  T* oldBuffer, int32 oldCapacity, int32 newCapacity)
{
  b2Assert(newCapacity > oldCapacity);
  T* newBuffer = (T*) m_world->m_blockAllocator.Allocate(
    sizeof(T) * newCapacity);
  if (oldBuffer)
  {
    memcpy(newBuffer, oldBuffer, sizeof(T) * oldCapacity);
    m_world->m_blockAllocator.Free(oldBuffer, sizeof(T) * oldCapacity);
  }
  return newBuffer;
}

// Reallocate a buffer
template <typename T> T* b2ParticleSystem::ReallocateBuffer(
  T* buffer, int32 userSuppliedCapacity, int32 oldCapacity,
  int32 newCapacity, bool deferred)
{
  b2Assert(newCapacity > oldCapacity);
  // A 'deferred' buffer is reallocated only if it is not NULL.
  // If 'userSuppliedCapacity' is not zero, buffer is user supplied and must
  // be kept.
  b2Assert(!userSuppliedCapacity || newCapacity <= userSuppliedCapacity);
  if ((!deferred || buffer) && !userSuppliedCapacity)
  {
    buffer = ReallocateBuffer(buffer, oldCapacity, newCapacity);
  }
  return buffer;
}

// Reallocate a buffer
template <typename T> T* b2ParticleSystem::ReallocateBuffer(
  UserOverridableBuffer<T>* buffer, int32 oldCapacity, int32 newCapacity,
  bool deferred)
{
  b2Assert(newCapacity > oldCapacity);
  return ReallocateBuffer(buffer->data, buffer->userSuppliedCapacity,
              oldCapacity, newCapacity, deferred);
}

/// Reallocate the handle / index map and schedule the allocation of a new
/// pool for handle allocation.
void b2ParticleSystem::ReallocateHandleBuffers(int32 newCapacity)
{
  b2Assert(newCapacity > m_internalAllocatedCapacity);
  // Reallocate a new handle / index map buffer, copying old handle pointers
  // is fine since they're kept around.
  m_handleIndexBuffer.data = ReallocateBuffer(
    &m_handleIndexBuffer, m_internalAllocatedCapacity, newCapacity,
    true);
  // Set the size of the next handle allocation.
  m_handleAllocator.SetItemsPerSlab(newCapacity -
                    m_internalAllocatedCapacity);
}

template <typename T> T* b2ParticleSystem::RequestBuffer(T* buffer)
{
  if (!buffer)
  {
    if (m_internalAllocatedCapacity == 0)
    {
      ReallocateInternalAllocatedBuffers(
        b2_minParticleSystemBufferCapacity);
    }
    buffer = (T*) (m_world->m_blockAllocator.Allocate(
               sizeof(T) * m_internalAllocatedCapacity));
    b2Assert(buffer);
    memset(buffer, 0, sizeof(T) * m_internalAllocatedCapacity);
  }
  return buffer;
}

b2ParticleColor* b2ParticleSystem::GetColorBuffer()
{
  m_colorBuffer.data = RequestBuffer(m_colorBuffer.data);
  return m_colorBuffer.data;
}

void** b2ParticleSystem::GetUserDataBuffer()
{
  m_userDataBuffer.data = RequestBuffer(m_userDataBuffer.data);
  return m_userDataBuffer.data;
}

static int32 LimitCapacity(int32 capacity, int32 maxCount)
{
  return maxCount && capacity > maxCount ? maxCount : capacity;
}

void b2ParticleSystem::ReallocateInternalAllocatedBuffers(int32 capacity)
{
  // Don't increase capacity beyond the smallest user-supplied buffer size.
  capacity = LimitCapacity(capacity, m_def.maxCount);
  capacity = LimitCapacity(capacity, m_flagsBuffer.userSuppliedCapacity);
  capacity = LimitCapacity(capacity, m_positionBuffer.userSuppliedCapacity);
  capacity = LimitCapacity(capacity, m_velocityBuffer.userSuppliedCapacity);
  capacity = LimitCapacity(capacity, m_colorBuffer.userSuppliedCapacity);
  capacity = LimitCapacity(capacity, m_userDataBuffer.userSuppliedCapacity);
  if (m_internalAllocatedCapacity < capacity)
  {
    ReallocateHandleBuffers(capacity);
    m_flagsBuffer.data = ReallocateBuffer(
      &m_flagsBuffer, m_internalAllocatedCapacity, capacity, false);

    // Conditionally defer these as they are optional if the feature is
    // not enabled.
    const bool stuck = m_stuckThreshold > 0;
    m_lastBodyContactStepBuffer.data = ReallocateBuffer(
      &m_lastBodyContactStepBuffer, m_internalAllocatedCapacity,
      capacity, stuck);
    m_bodyContactCountBuffer.data = ReallocateBuffer(
      &m_bodyContactCountBuffer, m_internalAllocatedCapacity, capacity,
      stuck);
    m_consecutiveContactStepsBuffer.data = ReallocateBuffer(
      &m_consecutiveContactStepsBuffer, m_internalAllocatedCapacity,
      capacity, stuck);
    m_positionBuffer.data = ReallocateBuffer(
      &m_positionBuffer, m_internalAllocatedCapacity, capacity, false);
    m_velocityBuffer.data = ReallocateBuffer(
      &m_velocityBuffer, m_internalAllocatedCapacity, capacity, false);
    m_forceBuffer = ReallocateBuffer(
      m_forceBuffer, 0, m_internalAllocatedCapacity, capacity, false);
    m_weightBuffer = ReallocateBuffer(
      m_weightBuffer, 0, m_internalAllocatedCapacity, capacity, false);
    m_staticPressureBuffer = ReallocateBuffer(
      m_staticPressureBuffer, 0, m_internalAllocatedCapacity, capacity,
      true);
    m_accumulationBuffer = ReallocateBuffer(
      m_accumulationBuffer, 0, m_internalAllocatedCapacity, capacity,
      false);
    m_accumulation2Buffer = ReallocateBuffer(
      m_accumulation2Buffer, 0, m_internalAllocatedCapacity, capacity,
      true);
    m_depthBuffer = ReallocateBuffer(
      m_depthBuffer, 0, m_internalAllocatedCapacity, capacity, true);
    m_colorBuffer.data = ReallocateBuffer(
      &m_colorBuffer, m_internalAllocatedCapacity, capacity, true);
    m_groupBuffer = ReallocateBuffer(
      m_groupBuffer, 0, m_internalAllocatedCapacity, capacity, false);
    m_userDataBuffer.data = ReallocateBuffer(
      &m_userDataBuffer, m_internalAllocatedCapacity, capacity, true);
    m_expirationTimeBuffer.data = ReallocateBuffer(
      &m_expirationTimeBuffer, m_internalAllocatedCapacity, capacity,
      true);
    m_indexByExpirationTimeBuffer.data = ReallocateBuffer(
      &m_indexByExpirationTimeBuffer, m_internalAllocatedCapacity,
      capacity, true);
    m_internalAllocatedCapacity = capacity;
  }
}

int32 b2ParticleSystem::CreateParticle(const b2ParticleDef& def)
{
  b2Assert(m_world->IsLocked() == false);
  if (m_world->IsLocked())
  {
    return 0;
  }

  if (m_count >= m_internalAllocatedCapacity)
  {
    // Double the particle capacity.
    int32 capacity =
      m_count ? 2 * m_count : b2_minParticleSystemBufferCapacity;
    ReallocateInternalAllocatedBuffers(capacity);
  }
  if (m_count >= m_internalAllocatedCapacity)
  {
    // If the oldest particle should be destroyed...
    if (m_def.destroyByAge)
    {
      DestroyOldestParticle(0, false);
      // Need to destroy this particle *now* so that it's possible to
      // create a new particle.
      SolveZombie();
    }
    else
    {
      return b2_invalidParticleIndex;
    }
  }
  int32 index = m_count++;
  m_flagsBuffer.data[index] = 0;
  if (m_lastBodyContactStepBuffer.data)
  {
    m_lastBodyContactStepBuffer.data[index] = 0;
  }
  if (m_bodyContactCountBuffer.data)
  {
    m_bodyContactCountBuffer.data[index] = 0;
  }
  if (m_consecutiveContactStepsBuffer.data)
  {
    m_consecutiveContactStepsBuffer.data[index] = 0;
  }
  m_positionBuffer.data[index] = def.position;
  m_velocityBuffer.data[index] = def.velocity;
  m_weightBuffer[index] = 0;
  m_forceBuffer[index] = b2Vec2_zero;
  if (m_staticPressureBuffer)
  {
    m_staticPressureBuffer[index] = 0;
  }
  if (m_depthBuffer)
  {
    m_depthBuffer[index] = 0;
  }
  if (m_colorBuffer.data || !def.color.IsZero())
  {
    m_colorBuffer.data = RequestBuffer(m_colorBuffer.data);
    m_colorBuffer.data[index] = def.color;
  }
  if (m_userDataBuffer.data || def.userData)
  {
    m_userDataBuffer.data= RequestBuffer(m_userDataBuffer.data);
    m_userDataBuffer.data[index] = def.userData;
  }
  if (m_handleIndexBuffer.data)
  {
    m_handleIndexBuffer.data[index] = NULL;
  }
  Proxy& proxy = m_proxyBuffer.Append();

  // If particle lifetimes are enabled or the lifetime is set in the particle
  // definition, initialize the lifetime.
  const bool finiteLifetime = def.lifetime > 0;
  if (m_expirationTimeBuffer.data || finiteLifetime)
  {
    SetParticleLifetime(index, finiteLifetime ? def.lifetime :
                ExpirationTimeToLifetime(
                  -GetQuantizedTimeElapsed()));
    // Add a reference to the newly added particle to the end of the
    // queue.
    m_indexByExpirationTimeBuffer.data[index] = index;
  }

  proxy.index = index;
  b2ParticleGroup* group = def.group;
  m_groupBuffer[index] = group;
  if (group)
  {
    if (group->m_firstIndex < group->m_lastIndex)
    {
      // Move particles in the group just before the new particle.
      RotateBuffer(group->m_firstIndex, group->m_lastIndex, index);
      b2Assert(group->m_lastIndex == index);
      // Update the index range of the group to contain the new particle.
      group->m_lastIndex = index + 1;
    }
    else
    {
      // If the group is empty, reset the index range to contain only the
      // new particle.
      group->m_firstIndex = index;
      group->m_lastIndex = index + 1;
    }
  }
  SetParticleFlags(index, def.flags);
  return index;
}

/// Retrieve a handle to the particle at the specified index.
const b2ParticleHandle* b2ParticleSystem::GetParticleHandleFromIndex(
  const int32 index)
{
  b2Assert(index >= 0 && index < GetParticleCount() &&
       index != b2_invalidParticleIndex);
  m_handleIndexBuffer.data = RequestBuffer(m_handleIndexBuffer.data);
  b2ParticleHandle* handle = m_handleIndexBuffer.data[index];
  if (handle)
  {
    return handle;
  }
  // Create a handle.
  handle = m_handleAllocator.Allocate();
  b2Assert(handle);
  handle->SetIndex(index);
  m_handleIndexBuffer.data[index] = handle;
  return handle;
}


void b2ParticleSystem::DestroyParticle(
  int32 index, bool callDestructionListener)
{
  uint32 flags = b2_zombieParticle;
  if (callDestructionListener)
  {
    flags |= b2_destructionListenerParticle;
  }
  SetParticleFlags(index, m_flagsBuffer.data[index] | flags);
}

void b2ParticleSystem::DestroyOldestParticle(
  const int32 index, const bool callDestructionListener)
{
  const int32 particleCount = GetParticleCount();
  b2Assert(index >= 0 && index < particleCount);
  // Make sure particle lifetime tracking is enabled.
  b2Assert(m_indexByExpirationTimeBuffer.data);
  // Destroy the oldest particle (preferring to destroy finite
  // lifetime particles first) to free a slot in the buffer.
  const int32 oldestFiniteLifetimeParticle =
    m_indexByExpirationTimeBuffer.data[particleCount - (index + 1)];
  const int32 oldestInfiniteLifetimeParticle =
    m_indexByExpirationTimeBuffer.data[index];
  DestroyParticle(
    m_expirationTimeBuffer.data[oldestFiniteLifetimeParticle] > 0.0f ?
      oldestFiniteLifetimeParticle : oldestInfiniteLifetimeParticle,
    callDestructionListener);
}

int32 b2ParticleSystem::DestroyParticlesInShape(
  const b2Shape& shape, const b2Transform& xf,
  bool callDestructionListener)
{
  b2Assert(m_world->IsLocked() == false);
  if (m_world->IsLocked())
  {
    return 0;
  }

  class DestroyParticlesInShapeCallback : public b2QueryCallback
  {
  public:
    DestroyParticlesInShapeCallback(
      b2ParticleSystem* system, const b2Shape& shape,
      const b2Transform& xf, bool callDestructionListener)
    {
      m_system = system;
      m_shape = &shape;
      m_xf = xf;
      m_callDestructionListener = callDestructionListener;
      m_destroyed = 0;
    }

    bool ReportFixture(b2Fixture* fixture)
    {
      B2_NOT_USED(fixture);
      return false;
    }

    bool ReportParticle(const b2ParticleSystem* particleSystem, int32 index)
    {
      if (particleSystem != m_system)
        return false;

      b2Assert(index >=0 && index < m_system->m_count);
      if (m_shape->TestPoint(m_xf,
                   m_system->m_positionBuffer.data[index]))
      {
        m_system->DestroyParticle(index, m_callDestructionListener);
        m_destroyed++;
      }
      return true;
    }

    int32 Destroyed() { return m_destroyed; }

  private:
    b2ParticleSystem* m_system;
    const b2Shape* m_shape;
    b2Transform m_xf;
    bool m_callDestructionListener;
    int32 m_destroyed;
  } callback(this, shape, xf, callDestructionListener);
  b2AABB aabb;
  shape.ComputeAABB(&aabb, xf);
  m_world->QueryAABB(&callback, aabb);
  return callback.Destroyed();
}

int32 b2ParticleSystem::CreateParticleForGroup(
  const b2ParticleGroupDef& groupDef, const b2Transform& xf, const b2Vec2& p)
{
  b2ParticleDef particleDef;
  particleDef.flags = groupDef.flags;
  particleDef.position = b2Mul(xf, p);
  particleDef.velocity =
    groupDef.linearVelocity +
    b2Cross(groupDef.angularVelocity,
        particleDef.position - groupDef.position);
  particleDef.color = groupDef.color;
  particleDef.lifetime = groupDef.lifetime;
  particleDef.userData = groupDef.userData;
  return CreateParticle(particleDef);
}

void b2ParticleSystem::CreateParticlesStrokeShapeForGroup(
  const b2Shape *shape,
  const b2ParticleGroupDef& groupDef, const b2Transform& xf)
{
  float32 stride = groupDef.stride;
  if (stride == 0)
  {
    stride = GetParticleStride();
  }
  float32 positionOnEdge = 0;
  b2EdgeShape edge;
  if (shape->GetType() == b2Shape::e_edge)
  {
    edge = *(b2EdgeShape*) shape;
  }
  else
  {
    //b2Assert(shape->GetType() == b2Shape::e_chain);
    // TODO
    //((b2ChainShape*) shape)->GetChildEdge(&edge, childIndex);
  }
  b2Vec2 d = edge.m_vertex2 - edge.m_vertex1;
  float32 edgeLength = d.Length();
  while (positionOnEdge < edgeLength)
  {
    b2Vec2 p = edge.m_vertex1 + positionOnEdge / edgeLength * d;
    CreateParticleForGroup(groupDef, xf, p);
    positionOnEdge += stride;
  }
  positionOnEdge -= edgeLength;
}

void b2ParticleSystem::CreateParticlesFillShapeForGroup(
  const b2Shape *shape,
  const b2ParticleGroupDef& groupDef, const b2Transform& xf)
{
  float32 stride = groupDef.stride;
  if (stride == 0)
  {
    stride = GetParticleStride();
  }
  b2Transform identity;
  identity.SetIdentity();
  b2AABB aabb;
  shape->ComputeAABB(&aabb, identity);
  for (float32 y = floorf(aabb.lowerBound.y / stride) * stride;
    y < aabb.upperBound.y; y += stride)
  {
    for (float32 x = floorf(aabb.lowerBound.x / stride) * stride;
      x < aabb.upperBound.x; x += stride)
    {
      b2Vec2 p(x, y);
      if (shape->TestPoint(identity, p))
      {
        CreateParticleForGroup(groupDef, xf, p);
      }
    }
  }
}

void b2ParticleSystem::CreateParticlesWithShapeForGroup(
  const b2Shape* shape,
  const b2ParticleGroupDef& groupDef, const b2Transform& xf)
{
  switch (shape->GetType()) {
  case b2Shape::e_edge:
  //case b2Shape::e_chain:
    CreateParticlesStrokeShapeForGroup(shape, groupDef, xf);
    break;
  case b2Shape::e_polygon:
  case b2Shape::e_circle:
    CreateParticlesFillShapeForGroup(shape, groupDef, xf);
    break;
  default:
    b2Assert(false);
    break;
  }
}

void b2ParticleSystem::CreateParticlesWithShapesForGroup(
  const b2Shape* const* shapes, int32 shapeCount,
  const b2ParticleGroupDef& groupDef, const b2Transform& xf)
{
  class CompositeShape : public b2Shape
  {
  public:
    CompositeShape(const b2Shape* const* shapes, int32 shapeCount)
    {
      m_shapes = shapes;
      m_shapeCount = shapeCount;
    }
    b2Shape* Clone(b2BlockAllocator* allocator) const
    {
      b2Assert(false);
      B2_NOT_USED(allocator);
      return NULL;
    }
    bool TestPoint(const b2Transform& xf, const b2Vec2& p) const
    {
      for (int32 i = 0; i < m_shapeCount; i++)
      {
        if (m_shapes[i]->TestPoint(xf, p))
        {
          return true;
        }
      }
      return false;
    }
    void ComputeDistance(const b2Transform& xf, const b2Vec2& p,
          float32* distance, b2Vec2* normal) const
    {
      b2Assert(false);
      B2_NOT_USED(xf);
      B2_NOT_USED(p);
      B2_NOT_USED(distance);
      B2_NOT_USED(normal);
    }
    bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
            const b2Transform& transform) const
    {
      b2Assert(false);
      B2_NOT_USED(output);
      B2_NOT_USED(input);
      B2_NOT_USED(transform);
      return false;
    }
    void ComputeAABB(
        b2AABB* aabb, const b2Transform& xf) const
    {
      aabb->lowerBound.x = +FLT_MAX;
      aabb->lowerBound.y = +FLT_MAX;
      aabb->upperBound.x = -FLT_MAX;
      aabb->upperBound.y = -FLT_MAX;
      for (int32 i = 0; i < m_shapeCount; i++)
      {
        b2AABB subaabb;
        m_shapes[i]->ComputeAABB(&subaabb, xf);
        aabb->Combine(subaabb);
      }
    }
    void ComputeMass(b2MassData* massData, float32 density) const
    {
      b2Assert(false);
      B2_NOT_USED(massData);
      B2_NOT_USED(density);
    }
  private:
    const b2Shape* const* m_shapes;
    int32 m_shapeCount;
  } compositeShape(shapes, shapeCount);
  CreateParticlesFillShapeForGroup(&compositeShape, groupDef, xf);
}

b2ParticleGroup* b2ParticleSystem::CreateParticleGroup(
  const b2ParticleGroupDef& groupDef)
{
  b2Assert(m_world->IsLocked() == false);
  if (m_world->IsLocked())
  {
    return 0;
  }

  b2Transform transform;
  transform.Set(groupDef.position, groupDef.angle);
  int32 firstIndex = m_count;
  if (groupDef.shape)
  {
    CreateParticlesWithShapeForGroup(groupDef.shape, groupDef, transform);
  }
  if (groupDef.shapes)
  {
    CreateParticlesWithShapesForGroup(
          groupDef.shapes, groupDef.shapeCount, groupDef, transform);
  }
  if (groupDef.particleCount)
  {
    b2Assert(groupDef.positionData);
    for (int32 i = 0; i < groupDef.particleCount; i++)
    {
      b2Vec2 p = groupDef.positionData[i];
      CreateParticleForGroup(groupDef, transform, p);
    }
  }
  int32 lastIndex = m_count;

  void* mem = m_world->m_blockAllocator.Allocate(sizeof(b2ParticleGroup));
  b2ParticleGroup* group = new (mem) b2ParticleGroup();
  group->m_system = this;
  group->m_firstIndex = firstIndex;
  group->m_lastIndex = lastIndex;
  group->m_strength = groupDef.strength;
  group->m_userData = groupDef.userData;
  group->m_transform = transform;
  group->m_prev = NULL;
  group->m_next = m_groupList;
  if (m_groupList)
  {
    m_groupList->m_prev = group;
  }
  m_groupList = group;
  ++m_groupCount;
  for (int32 i = firstIndex; i < lastIndex; i++)
  {
    m_groupBuffer[i] = group;
  }
  SetGroupFlags(group, groupDef.groupFlags);

  // Create pairs and triads between particles in the group.
  ConnectionFilter filter;
  UpdateContacts(true);
  UpdatePairsAndTriads(firstIndex, lastIndex, filter);

  if (groupDef.group)
  {
    JoinParticleGroups(groupDef.group, group);
    group = groupDef.group;
  }

  return group;
}

void b2ParticleSystem::JoinParticleGroups(b2ParticleGroup* groupA,
                      b2ParticleGroup* groupB)
{
  b2Assert(m_world->IsLocked() == false);
  if (m_world->IsLocked())
  {
    return;
  }

  b2Assert(groupA != groupB);
  RotateBuffer(groupB->m_firstIndex, groupB->m_lastIndex, m_count);
  b2Assert(groupB->m_lastIndex == m_count);
  RotateBuffer(groupA->m_firstIndex, groupA->m_lastIndex,
         groupB->m_firstIndex);
  b2Assert(groupA->m_lastIndex == groupB->m_firstIndex);

  // Create pairs and triads connecting groupA and groupB.
  class JoinParticleGroupsFilter : public ConnectionFilter
  {
    bool ShouldCreatePair(int32 a, int32 b) const
    {
      return
        (a < m_threshold && m_threshold <= b) ||
        (b < m_threshold && m_threshold <= a);
    }
    bool ShouldCreateTriad(int32 a, int32 b, int32 c) const
    {
      return
        (a < m_threshold || b < m_threshold || c < m_threshold) &&
        (m_threshold <= a || m_threshold <= b || m_threshold <= c);
    }
    int32 m_threshold;
  public:
    JoinParticleGroupsFilter(int32 threshold)
    {
      m_threshold = threshold;
    }
  } filter(groupB->m_firstIndex);
  UpdateContacts(true);
  UpdatePairsAndTriads(groupA->m_firstIndex, groupB->m_lastIndex, filter);

  for (int32 i = groupB->m_firstIndex; i < groupB->m_lastIndex; i++)
  {
    m_groupBuffer[i] = groupA;
  }
  uint32 groupFlags = groupA->m_groupFlags | groupB->m_groupFlags;
  SetGroupFlags(groupA, groupFlags);
  groupA->m_lastIndex = groupB->m_lastIndex;
  groupB->m_firstIndex = groupB->m_lastIndex;
  DestroyParticleGroup(groupB);
}

void b2ParticleSystem::SplitParticleGroup(b2ParticleGroup* group)
{
  UpdateContacts(true);
  int32 particleCount = group->GetParticleCount();
  // We create several linked lists. Each list represents a set of connected
  // particles.
  ParticleListNode* nodeBuffer =
    (ParticleListNode*) m_world->m_stackAllocator.Allocate(
                  sizeof(ParticleListNode) * particleCount);
  InitializeParticleLists(group, nodeBuffer);
  MergeParticleListsInContact(group, nodeBuffer);
  ParticleListNode* survivingList =
                  FindLongestParticleList(group, nodeBuffer);
  MergeZombieParticleListNodes(group, nodeBuffer, survivingList);
  CreateParticleGroupsFromParticleList(group, nodeBuffer, survivingList);
  UpdatePairsAndTriadsWithParticleList(group, nodeBuffer);
  m_world->m_stackAllocator.Free(nodeBuffer);
}

void b2ParticleSystem::InitializeParticleLists(
  const b2ParticleGroup* group, ParticleListNode* nodeBuffer)
{
  int32 bufferIndex = group->GetBufferIndex();
  int32 particleCount = group->GetParticleCount();
  for (int32 i = 0; i < particleCount; i++)
  {
    ParticleListNode* node = &nodeBuffer[i];
    node->list = node;
    node->next = NULL;
    node->count = 1;
    node->index = i + bufferIndex;
  }
}

void b2ParticleSystem::MergeParticleListsInContact(
  const b2ParticleGroup* group, ParticleListNode* nodeBuffer) const
{
  int32 bufferIndex = group->GetBufferIndex();
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    int32 a = contact.GetIndexA();
    int32 b = contact.GetIndexB();
    if (!group->ContainsParticle(a) || !group->ContainsParticle(b)) {
      continue;
    }
    ParticleListNode* listA = nodeBuffer[a - bufferIndex].list;
    ParticleListNode* listB = nodeBuffer[b - bufferIndex].list;
    if (listA == listB) {
      continue;
    }
    // To minimize the cost of insertion, make sure listA is longer than
    // listB.
    if (listA->count < listB->count)
    {
      b2Swap(listA, listB);
    }
    b2Assert(listA->count >= listB->count);
    MergeParticleLists(listA, listB);
  }
}

void b2ParticleSystem::MergeParticleLists(
  ParticleListNode* listA, ParticleListNode* listB)
{
  // Insert listB between index 0 and 1 of listA
  // Example:
  //     listA => a1 => a2 => a3 => NULL
  //     listB => b1 => b2 => NULL
  // to
  //     listA => listB => b1 => b2 => a1 => a2 => a3 => NULL
  b2Assert(listA != listB);
  for (ParticleListNode* b = listB;;)
  {
    b->list = listA;
    ParticleListNode* nextB = b->next;
    if (nextB)
    {
      b = nextB;
    }
    else
    {
      b->next = listA->next;
      break;
    }
  }
  listA->next = listB;
  listA->count += listB->count;
  listB->count = 0;
}

b2ParticleSystem::ParticleListNode* b2ParticleSystem::FindLongestParticleList(
  const b2ParticleGroup* group, ParticleListNode* nodeBuffer)
{
  int32 particleCount = group->GetParticleCount();
  ParticleListNode* result = nodeBuffer;
  for (int32 i = 0; i < particleCount; i++)
  {
    ParticleListNode* node = &nodeBuffer[i];
    if (result->count < node->count)
    {
      result = node;
    }
  }
  return result;
}

void b2ParticleSystem::MergeZombieParticleListNodes(
  const b2ParticleGroup* group, ParticleListNode* nodeBuffer,
  ParticleListNode* survivingList) const
{
  int32 particleCount = group->GetParticleCount();
  for (int32 i = 0; i < particleCount; i++)
  {
    ParticleListNode* node = &nodeBuffer[i];
    if (node != survivingList &&
      (m_flagsBuffer.data[node->index] & b2_zombieParticle))
    {
      MergeParticleListAndNode(survivingList, node);
    }
  }
}

void b2ParticleSystem::MergeParticleListAndNode(
  ParticleListNode* list, ParticleListNode* node)
{
  // Insert node between index 0 and 1 of list
  // Example:
  //     list => a1 => a2 => a3 => NULL
  //     node => NULL
  // to
  //     list => node => a1 => a2 => a3 => NULL
  b2Assert(node != list);
  b2Assert(node->list == node);
  b2Assert(node->count == 1);
  node->list = list;
  node->next = list->next;
  list->next = node;
  list->count++;
  node->count = 0;
}

void b2ParticleSystem::CreateParticleGroupsFromParticleList(
  const b2ParticleGroup* group, ParticleListNode* nodeBuffer,
  const ParticleListNode* survivingList)
{
  int32 particleCount = group->GetParticleCount();
  b2ParticleGroupDef def;
  def.groupFlags = group->GetGroupFlags();
  def.userData = group->GetUserData();
  for (int32 i = 0; i < particleCount; i++)
  {
    ParticleListNode* list = &nodeBuffer[i];
    if (!list->count || list == survivingList)
    {
      continue;
    }
    b2Assert(list->list == list);
    b2ParticleGroup* newGroup = CreateParticleGroup(def);
    for (ParticleListNode* node = list; node; node = node->next)
    {
      int32 oldIndex = node->index;
      b2Assert(!(m_flagsBuffer.data[oldIndex] & b2_zombieParticle));
      int32 newIndex = CloneParticle(oldIndex, newGroup);
      m_flagsBuffer.data[oldIndex] |= b2_zombieParticle;
      node->index = newIndex;
    }
  }
}

void b2ParticleSystem::UpdatePairsAndTriadsWithParticleList(
  const b2ParticleGroup* group, const ParticleListNode* nodeBuffer)
{
  int32 bufferIndex = group->GetBufferIndex();
  // Update indices in pairs and triads. If an index belongs to the group,
  // replace it with the corresponding value in nodeBuffer.
  // Note that nodeBuffer is allocated only for the group and the index should
  // be shifted by bufferIndex.
  for (int32 k = 0; k < m_pairBuffer.GetCount(); k++)
  {
    b2ParticlePair& pair = m_pairBuffer[k];
    int32 a = pair.indexA;
    int32 b = pair.indexB;
    if (group->ContainsParticle(a))
    {
      pair.indexA = nodeBuffer[a - bufferIndex].index;
    }
    if (group->ContainsParticle(b))
    {
      pair.indexB = nodeBuffer[b - bufferIndex].index;
    }
  }
  for (int32 k = 0; k < m_triadBuffer.GetCount(); k++)
  {
    b2ParticleTriad& triad = m_triadBuffer[k];
    int32 a = triad.indexA;
    int32 b = triad.indexB;
    int32 c = triad.indexC;
    if (group->ContainsParticle(a))
    {
      triad.indexA = nodeBuffer[a - bufferIndex].index;
    }
    if (group->ContainsParticle(b))
    {
      triad.indexB = nodeBuffer[b - bufferIndex].index;
    }
    if (group->ContainsParticle(c))
    {
      triad.indexC = nodeBuffer[c - bufferIndex].index;
    }
  }
}

int32 b2ParticleSystem::CloneParticle(int32 oldIndex, b2ParticleGroup* group)
{
  b2ParticleDef def;
  def.flags = m_flagsBuffer.data[oldIndex];
  def.position = m_positionBuffer.data[oldIndex];
  def.velocity = m_velocityBuffer.data[oldIndex];
  if (m_colorBuffer.data)
  {
    def.color = m_colorBuffer.data[oldIndex];
  }
  if (m_userDataBuffer.data)
  {
    def.userData = m_userDataBuffer.data[oldIndex];
  }
  def.group = group;
  int32 newIndex = CreateParticle(def);
  if (m_handleIndexBuffer.data)
  {
    b2ParticleHandle* handle = m_handleIndexBuffer.data[oldIndex];
    if (handle) handle->SetIndex(newIndex);
    m_handleIndexBuffer.data[newIndex] = handle;
    m_handleIndexBuffer.data[oldIndex] = NULL;
  }
  if (m_lastBodyContactStepBuffer.data)
  {
    m_lastBodyContactStepBuffer.data[newIndex] =
      m_lastBodyContactStepBuffer.data[oldIndex];
  }
  if (m_bodyContactCountBuffer.data)
  {
    m_bodyContactCountBuffer.data[newIndex] =
      m_bodyContactCountBuffer.data[oldIndex];
  }
  if (m_consecutiveContactStepsBuffer.data)
  {
    m_consecutiveContactStepsBuffer.data[newIndex] =
      m_consecutiveContactStepsBuffer.data[oldIndex];
  }
  if (m_hasForce)
  {
    m_forceBuffer[newIndex] = m_forceBuffer[oldIndex];
  }
  if (m_staticPressureBuffer)
  {
    m_staticPressureBuffer[newIndex] = m_staticPressureBuffer[oldIndex];
  }
  if (m_depthBuffer)
  {
    m_depthBuffer[newIndex] = m_depthBuffer[oldIndex];
  }
  if (m_expirationTimeBuffer.data)
  {
    m_expirationTimeBuffer.data[newIndex] =
      m_expirationTimeBuffer.data[oldIndex];
  }
  return newIndex;
}

void b2ParticleSystem::UpdatePairsAndTriadsWithReactiveParticles()
{
  class ReactiveFilter : public ConnectionFilter
  {
    bool IsNecessary(int32 index) const
    {
      return (m_flagsBuffer[index] & b2_reactiveParticle) != 0;
    }
    const uint32* m_flagsBuffer;
  public:
    ReactiveFilter(uint32* flagsBuffer)
    {
      m_flagsBuffer = flagsBuffer;
    }
  } filter(m_flagsBuffer.data);
  UpdatePairsAndTriads(0, m_count, filter);

  for (int32 i = 0; i < m_count; i++)
  {
    m_flagsBuffer.data[i] &= ~b2_reactiveParticle;
  }
  m_allParticleFlags &= ~b2_reactiveParticle;
}

static bool ParticleCanBeConnected(
  uint32 flags, b2ParticleGroup* group)
{
  return
    (flags & (b2_wallParticle | b2_springParticle | b2_elasticParticle)) ||
    (group && group->GetGroupFlags() & b2_rigidParticleGroup);
}

void b2ParticleSystem::UpdatePairsAndTriads(
  int32 firstIndex, int32 lastIndex, const ConnectionFilter& filter)
{
  // Create pairs or triads.
  // All particles in each pair/triad should satisfy the following:
  // * firstIndex <= index < lastIndex
  // * don't have b2_zombieParticle
  // * ParticleCanBeConnected returns true
  // * ShouldCreatePair/ShouldCreateTriad returns true
  // Any particles in each pair/triad should satisfy the following:
  // * filter.IsNeeded returns true
  // * have one of k_pairFlags/k_triadsFlags
  b2Assert(firstIndex <= lastIndex);
  uint32 particleFlags = 0;
  for (int32 i = firstIndex; i < lastIndex; i++)
  {
    particleFlags |= m_flagsBuffer.data[i];
  }
  if (particleFlags & k_pairFlags)
  {
    for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
    {
      const b2ParticleContact& contact = m_contactBuffer[k];
      int32 a = contact.GetIndexA();
      int32 b = contact.GetIndexB();
      uint32 af = m_flagsBuffer.data[a];
      uint32 bf = m_flagsBuffer.data[b];
      b2ParticleGroup* groupA = m_groupBuffer[a];
      b2ParticleGroup* groupB = m_groupBuffer[b];
      if (a >= firstIndex && a < lastIndex &&
        b >= firstIndex && b < lastIndex &&
        !((af | bf) & b2_zombieParticle) &&
        ((af | bf) & k_pairFlags) &&
        (filter.IsNecessary(a) || filter.IsNecessary(b)) &&
        ParticleCanBeConnected(af, groupA) &&
        ParticleCanBeConnected(bf, groupB) &&
        filter.ShouldCreatePair(a, b))
      {
        b2ParticlePair& pair = m_pairBuffer.Append();
        pair.indexA = a;
        pair.indexB = b;
        pair.flags = contact.GetFlags();
        pair.strength = b2Min(
          groupA ? groupA->m_strength : 1,
          groupB ? groupB->m_strength : 1);
        pair.distance = b2Distance(m_positionBuffer.data[a],
                       m_positionBuffer.data[b]);
      }
    }
    std::stable_sort(
      m_pairBuffer.Begin(), m_pairBuffer.End(), ComparePairIndices);
    m_pairBuffer.Unique(MatchPairIndices);
  }
  if (particleFlags & k_triadFlags)
  {
    b2VoronoiDiagram diagram(
      &m_world->m_stackAllocator, lastIndex - firstIndex);
    for (int32 i = firstIndex; i < lastIndex; i++)
    {
      uint32 flags = m_flagsBuffer.data[i];
      b2ParticleGroup* group = m_groupBuffer[i];
      if (!(flags & b2_zombieParticle) &&
        ParticleCanBeConnected(flags, group))
      {
        diagram.AddGenerator(
          m_positionBuffer.data[i], i, filter.IsNecessary(i));
      }
    }
    float32 stride = GetParticleStride();
    diagram.Generate(stride / 2, stride * 2);
    class UpdateTriadsCallback : public b2VoronoiDiagram::NodeCallback
    {
      void operator()(int32 a, int32 b, int32 c)
      {
        uint32 af = m_system->m_flagsBuffer.data[a];
        uint32 bf = m_system->m_flagsBuffer.data[b];
        uint32 cf = m_system->m_flagsBuffer.data[c];
        if (((af | bf | cf) & k_triadFlags) &&
          m_filter->ShouldCreateTriad(a, b, c))
        {
          const b2Vec2& pa = m_system->m_positionBuffer.data[a];
          const b2Vec2& pb = m_system->m_positionBuffer.data[b];
          const b2Vec2& pc = m_system->m_positionBuffer.data[c];
          b2Vec2 dab = pa - pb;
          b2Vec2 dbc = pb - pc;
          b2Vec2 dca = pc - pa;
          float32 maxDistanceSquared = b2_maxTriadDistanceSquared *
                         m_system->m_squaredDiameter;
          if (b2Dot(dab, dab) > maxDistanceSquared ||
            b2Dot(dbc, dbc) > maxDistanceSquared ||
            b2Dot(dca, dca) > maxDistanceSquared)
          {
            return;
          }
          b2ParticleGroup* groupA = m_system->m_groupBuffer[a];
          b2ParticleGroup* groupB = m_system->m_groupBuffer[b];
          b2ParticleGroup* groupC = m_system->m_groupBuffer[c];
          b2ParticleTriad& triad = m_system->m_triadBuffer.Append();
          triad.indexA = a;
          triad.indexB = b;
          triad.indexC = c;
          triad.flags = af | bf | cf;
          triad.strength = b2Min(b2Min(
            groupA ? groupA->m_strength : 1,
            groupB ? groupB->m_strength : 1),
            groupC ? groupC->m_strength : 1);
          b2Vec2 midPoint = (float32) 1 / 3 * (pa + pb + pc);
          triad.pa = pa - midPoint;
          triad.pb = pb - midPoint;
          triad.pc = pc - midPoint;
          triad.ka = -b2Dot(dca, dab);
          triad.kb = -b2Dot(dab, dbc);
          triad.kc = -b2Dot(dbc, dca);
          triad.s = b2Cross(pa, pb) + b2Cross(pb, pc) + b2Cross(pc, pa);
        }
      }
      b2ParticleSystem* m_system;
      const ConnectionFilter* m_filter;
    public:
      UpdateTriadsCallback(
        b2ParticleSystem* system, const ConnectionFilter* filter)
      {
        m_system = system;
        m_filter = filter;
      }
    } callback(this, &filter);
    diagram.GetNodes(callback);
    std::stable_sort(
      m_triadBuffer.Begin(), m_triadBuffer.End(), CompareTriadIndices);
    m_triadBuffer.Unique(MatchTriadIndices);
  }
}

bool b2ParticleSystem::ComparePairIndices(
              const b2ParticlePair& a, const b2ParticlePair& b)
{
  int32 diffA = a.indexA - b.indexA;
  if (diffA != 0) return diffA < 0;
  return a.indexB < b.indexB;
}

bool b2ParticleSystem::MatchPairIndices(
              const b2ParticlePair& a, const b2ParticlePair& b)
{
  return a.indexA == b.indexA && a.indexB == b.indexB;
}

bool b2ParticleSystem::CompareTriadIndices(
              const b2ParticleTriad& a, const b2ParticleTriad& b)
{
  int32 diffA = a.indexA - b.indexA;
  if (diffA != 0) return diffA < 0;
  int32 diffB = a.indexB - b.indexB;
  if (diffB != 0) return diffB < 0;
  return a.indexC < b.indexC;
}

bool b2ParticleSystem::MatchTriadIndices(
              const b2ParticleTriad& a, const b2ParticleTriad& b)
{
  return a.indexA == b.indexA && a.indexB == b.indexB && a.indexC == b.indexC;
}

// Only called from SolveZombie() or JoinParticleGroups().
void b2ParticleSystem::DestroyParticleGroup(b2ParticleGroup* group)
{
  b2Assert(m_groupCount > 0);
  b2Assert(group);

  if (m_world->m_destructionListener)
  {
    m_world->m_destructionListener->SayGoodbye(group);
  }

  SetGroupFlags(group, 0);
  for (int32 i = group->m_firstIndex; i < group->m_lastIndex; i++)
  {
    m_groupBuffer[i] = NULL;
  }

  if (group->m_prev)
  {
    group->m_prev->m_next = group->m_next;
  }
  if (group->m_next)
  {
    group->m_next->m_prev = group->m_prev;
  }
  if (group == m_groupList)
  {
    m_groupList = group->m_next;
  }

  --m_groupCount;
  group->~b2ParticleGroup();
  m_world->m_blockAllocator.Free(group, sizeof(b2ParticleGroup));
}

void b2ParticleSystem::ComputeWeight()
{
  // calculates the sum of contact-weights for each particle
  // that means dimensionless density
  memset(m_weightBuffer, 0, sizeof(*m_weightBuffer) * m_count);
  for (int32 k = 0; k < m_bodyContactBuffer.GetCount(); k++)
  {
    const b2ParticleBodyContact& contact = m_bodyContactBuffer[k];
    int32 a = contact.index;
    float32 w = contact.weight;
    m_weightBuffer[a] += w;
  }
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    int32 a = contact.GetIndexA();
    int32 b = contact.GetIndexB();
    float32 w = contact.GetWeight();
    m_weightBuffer[a] += w;
    m_weightBuffer[b] += w;
  }
}

void b2ParticleSystem::ComputeDepth()
{
  b2ParticleContact* contactGroups = (b2ParticleContact*) m_world->
    m_stackAllocator.Allocate(sizeof(b2ParticleContact) * m_contactBuffer.GetCount());
  int32 contactGroupsCount = 0;
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    int32 a = contact.GetIndexA();
    int32 b = contact.GetIndexB();
    const b2ParticleGroup* groupA = m_groupBuffer[a];
    const b2ParticleGroup* groupB = m_groupBuffer[b];
    if (groupA && groupA == groupB &&
      (groupA->m_groupFlags & b2_particleGroupNeedsUpdateDepth))
    {
      contactGroups[contactGroupsCount++] = contact;
    }
  }
  b2ParticleGroup** groupsToUpdate = (b2ParticleGroup**) m_world->
    m_stackAllocator.Allocate(sizeof(b2ParticleGroup*) * m_groupCount);
  int32 groupsToUpdateCount = 0;
  for (b2ParticleGroup* group = m_groupList; group; group = group->GetNext())
  {
    if (group->m_groupFlags & b2_particleGroupNeedsUpdateDepth)
    {
      groupsToUpdate[groupsToUpdateCount++] = group;
      SetGroupFlags(group,
              group->m_groupFlags &
              ~b2_particleGroupNeedsUpdateDepth);
      for (int32 i = group->m_firstIndex; i < group->m_lastIndex; i++)
      {
        m_accumulationBuffer[i] = 0;
      }
    }
  }
  // Compute sum of weight of contacts except between different groups.
  for (int32 k = 0; k < contactGroupsCount; k++)
  {
    const b2ParticleContact& contact = contactGroups[k];
    int32 a = contact.GetIndexA();
    int32 b = contact.GetIndexB();
    float32 w = contact.GetWeight();
    m_accumulationBuffer[a] += w;
    m_accumulationBuffer[b] += w;
  }
  b2Assert(m_depthBuffer);
  for (int32 i = 0; i < groupsToUpdateCount; i++)
  {
    const b2ParticleGroup* group = groupsToUpdate[i];
    for (int32 i = group->m_firstIndex; i < group->m_lastIndex; i++)
    {
      float32 w = m_accumulationBuffer[i];
      m_depthBuffer[i] = w < 0.8f ? 0 : b2_maxFloat;
    }
  }
  // The number of iterations is equal to particle number from the deepest
  // particle to the nearest surface particle, and in general it is smaller
  // than sqrt of total particle number.
  int32 iterationCount = (int32)b2Sqrt((float)m_count);
  for (int32 t = 0; t < iterationCount; t++)
  {
    bool updated = false;
    for (int32 k = 0; k < contactGroupsCount; k++)
    {
      const b2ParticleContact& contact = contactGroups[k];
      int32 a = contact.GetIndexA();
      int32 b = contact.GetIndexB();
      float32 r = 1 - contact.GetWeight();
      float32& ap0 = m_depthBuffer[a];
      float32& bp0 = m_depthBuffer[b];
      float32 ap1 = bp0 + r;
      float32 bp1 = ap0 + r;
      if (ap0 > ap1)
      {
        ap0 = ap1;
        updated = true;
      }
      if (bp0 > bp1)
      {
        bp0 = bp1;
        updated = true;
      }
    }
    if (!updated)
    {
      break;
    }
  }
  for (int32 i = 0; i < groupsToUpdateCount; i++)
  {
    const b2ParticleGroup* group = groupsToUpdate[i];
    for (int32 i = group->m_firstIndex; i < group->m_lastIndex; i++)
    {
      float32& p = m_depthBuffer[i];
      if (p < b2_maxFloat)
      {
        p *= m_particleDiameter;
      }
      else
      {
        p = 0;
      }
    }
  }
  m_world->m_stackAllocator.Free(groupsToUpdate);
  m_world->m_stackAllocator.Free(contactGroups);
}

b2ParticleSystem::InsideBoundsEnumerator
b2ParticleSystem::GetInsideBoundsEnumerator(const b2AABB& aabb) const
{
  uint32 lowerTag = computeTag(m_inverseDiameter * aabb.lowerBound.x - 1,
                 m_inverseDiameter * aabb.lowerBound.y - 1);
  uint32 upperTag = computeTag(m_inverseDiameter * aabb.upperBound.x + 1,
                 m_inverseDiameter * aabb.upperBound.y + 1);
  const Proxy* beginProxy = m_proxyBuffer.Begin();
  const Proxy* endProxy = m_proxyBuffer.End();
  const Proxy* firstProxy = std::lower_bound(beginProxy, endProxy, lowerTag);
  const Proxy* lastProxy = std::upper_bound(firstProxy, endProxy, upperTag);
  return InsideBoundsEnumerator(lowerTag, upperTag, firstProxy, lastProxy);
}

inline void b2ParticleSystem::AddContact(int32 a, int32 b,
  b2GrowableBuffer<b2ParticleContact>& contacts) const
{
  b2Vec2 d = m_positionBuffer.data[b] - m_positionBuffer.data[a];
  float32 distBtParticlesSq = b2Dot(d, d);
  if (distBtParticlesSq < m_squaredDiameter)
  {
    float32 invD = b2InvSqrt(distBtParticlesSq);
    b2ParticleContact& contact = contacts.Append();
    contact.SetIndices(a, b);
    contact.SetFlags(m_flagsBuffer.data[a] | m_flagsBuffer.data[b]);
    // 1 - distBtParticles / diameter
    contact.SetWeight(1 - distBtParticlesSq * invD * m_inverseDiameter);
    contact.SetNormal(invD * d);
  }
}

void b2ParticleSystem::FindContacts(
  b2GrowableBuffer<b2ParticleContact>& contacts) const
{
  const Proxy* beginProxy = m_proxyBuffer.Begin();
  const Proxy* endProxy = m_proxyBuffer.End();

  contacts.SetCount(0);
  for (const Proxy *a = beginProxy, *c = beginProxy; a < endProxy; a++)
  {
    uint32 rightTag = computeRelativeTag(a->tag, 1, 0);
    for (const Proxy* b = a + 1; b < endProxy; b++)
    {
      if (rightTag < b->tag) break;
      AddContact(a->index, b->index, contacts);
    }
    uint32 bottomLeftTag = computeRelativeTag(a->tag, -1, 1);
    for (; c < endProxy; c++)
    {
      if (bottomLeftTag <= c->tag) break;
    }
    uint32 bottomRightTag = computeRelativeTag(a->tag, 1, 1);
    for (const Proxy* b = c; b < endProxy; b++)
    {
      if (bottomRightTag < b->tag) break;
      AddContact(a->index, b->index, contacts);
    }
  }
}

static inline bool b2ParticleContactIsZombie(const b2ParticleContact& contact)
{
  return (contact.GetFlags() & b2_zombieParticle) == b2_zombieParticle;
}

// Get the world's contact filter if any particles with the
// b2_particleContactFilterParticle flag are present in the system.
inline b2ContactFilter* b2ParticleSystem::GetParticleContactFilter() const
{
  return (m_allParticleFlags & b2_particleContactFilterParticle) ?
    m_world->m_contactManager.m_contactFilter : NULL;
}

// Get the world's contact listener if any particles with the
// b2_particleContactListenerParticle flag are present in the system.
inline b2ContactListener* b2ParticleSystem::GetParticleContactListener() const
{
  return (m_allParticleFlags & b2_particleContactListenerParticle) ?
    m_world->m_contactManager.m_contactListener : NULL;
}

// Recalculate 'tag' in proxies using m_positionBuffer.
// The 'tag' is an approximation of position, in left-right, top-bottom order.
void b2ParticleSystem::UpdateProxies(
  b2GrowableBuffer<Proxy>& proxies) const
{
  const Proxy* const endProxy = proxies.End();
  for (Proxy* proxy = proxies.Begin(); proxy < endProxy; ++proxy)
  {
    int32 i = proxy->index;
    b2Vec2 p = m_positionBuffer.data[i];
    proxy->tag = computeTag(m_inverseDiameter * p.x,
                m_inverseDiameter * p.y);
  }
}

// static
bool b2ParticleSystem::ProxyBufferHasIndex(
  int32 index, const Proxy* const a, int count)
{
  for (int j = 0; j < count; ++j)
  {
    if (a[j].index == index)
      return true;
  }
  return false;
}

// static
int b2ParticleSystem::NumProxiesWithSameTag(
  const Proxy* const a, const Proxy* const b, int count)
{
  const uint32 tag = a[0].tag;
  for (int num = 0; num < count; ++num)
  {
    if (a[num].tag != tag || b[num].tag != tag)
      return num;
  }
  return count;
}

// Precondition: both 'a' and 'b' should be sorted by tag, but don't need to be
// sorted by index.
// static
bool b2ParticleSystem::AreProxyBuffersTheSame(const b2GrowableBuffer<Proxy>& a,
                          const b2GrowableBuffer<Proxy>& b)
{
  if (a.GetCount() != b.GetCount())
    return false;

  // A given tag may have several indices. The order of these indices is
  // not important, but the set must be equivalent.
  for (int i = 0; i < a.GetCount();)
  {
    const int numWithSameTag = NumProxiesWithSameTag(
      &a[i], &b[i], a.GetCount() - i);
    if (numWithSameTag == 0)
      return false;

    for (int j = 0; j < numWithSameTag; ++j)
    {
      const bool hasIndex = ProxyBufferHasIndex(
        a[i + j].index, &b[i], numWithSameTag);
      if (!hasIndex)
        return false;
    }

    i += numWithSameTag;
  }
  return true;
}

// Sort the proxy array by 'tag'. This orders the particles into rows that
// run left-to-right, top-to-bottom. The rows are spaced m_particleDiameter
// apart, such that a particle in one row can only collide with the rows
// immediately above and below it. This ordering makes collision computation
// tractable.
//
// TODO OPT: The sort is a hot spot on the profiles. We could use SIMD to
// speed this up. See http://www.vldb.org/pvldb/1/1454171.pdf for an excellent
// explanation of a SIMD mergesort algorithm.
void b2ParticleSystem::SortProxies(b2GrowableBuffer<Proxy>& proxies) const
{
  std::sort(proxies.Begin(), proxies.End());
}

class b2ParticleContactRemovePredicate
{
public:
  b2ParticleContactRemovePredicate(
    b2ParticleSystem* system,
    b2ContactFilter* contactFilter) :
    m_system(system),
    m_contactFilter(contactFilter)
  {}

  bool operator()(const b2ParticleContact& contact)
  {
      return (contact.GetFlags() & b2_particleContactFilterParticle)
          && !m_contactFilter->ShouldCollide(m_system, contact.GetIndexA(),
                             contact.GetIndexB());
  }

private:
  b2ParticleSystem* m_system;
  b2ContactFilter* m_contactFilter;
};

// Only changes 'contacts', but the contact filter has a non-const 'this'
// pointer, so this member function cannot be const.
void b2ParticleSystem::FilterContacts(
  b2GrowableBuffer<b2ParticleContact>& contacts)
{
  // Optionally filter the contact.
  b2ContactFilter* const contactFilter = GetParticleContactFilter();
  if (contactFilter == NULL)
    return;

  contacts.RemoveIf(b2ParticleContactRemovePredicate(this, contactFilter));
}

void b2ParticleSystem::NotifyContactListenerPreContact(
  b2ParticlePairSet* particlePairs) const
{
  b2ContactListener* const contactListener = GetParticleContactListener();
  if (contactListener == NULL)
    return;

  particlePairs->Initialize(m_contactBuffer.Begin(),
                m_contactBuffer.GetCount(),
                  GetFlagsBuffer());
}

// Note: This function is not const because 'this' in BeginContact and
// EndContact callbacks must be non-const. However, this function itself
// does not change any internal data (though the callbacks might).
void b2ParticleSystem::NotifyContactListenerPostContact(
  b2ParticlePairSet& particlePairs)
{
  b2ContactListener* const contactListener = GetParticleContactListener();
  if (contactListener == NULL)
    return;

  // Loop through all new contacts, reporting any new ones, and
  // "invalidating" the ones that still exist.
  const b2ParticleContact* const endContact = m_contactBuffer.End();
  for (b2ParticleContact* contact = m_contactBuffer.Begin();
     contact < endContact; ++contact)
  {
    ParticlePair pair;
    pair.first = contact->GetIndexA();
    pair.second = contact->GetIndexB();
    const int32 itemIndex = particlePairs.Find(pair);
    if (itemIndex >= 0)
    {
      // Already touching, ignore this contact.
      particlePairs.Invalidate(itemIndex);
    }
    else
    {
      // Just started touching, inform the listener.
      contactListener->BeginContact(this, contact);
    }
  }

  // Report particles that are no longer touching.
  // That is, any pairs that were not invalidated above.
  const int32 pairCount = particlePairs.GetCount();
  const ParticlePair* const pairs = particlePairs.GetBuffer();
  const int8* const valid = particlePairs.GetValidBuffer();
  for (int32 i = 0; i < pairCount; ++i)
  {
    if (valid[i])
    {
      contactListener->EndContact(this, pairs[i].first,
                    pairs[i].second);
    }
  }
}

void b2ParticleSystem::UpdateContacts(bool exceptZombie)
{
  UpdateProxies(m_proxyBuffer);
  SortProxies(m_proxyBuffer);

  b2ParticlePairSet particlePairs(&m_world->m_stackAllocator);
  NotifyContactListenerPreContact(&particlePairs);

  FindContacts(m_contactBuffer);
  FilterContacts(m_contactBuffer);

  NotifyContactListenerPostContact(particlePairs);

  if (exceptZombie)
  {
    m_contactBuffer.RemoveIf(b2ParticleContactIsZombie);
  }
}

void b2ParticleSystem::DetectStuckParticle(int32 particle)
{
  // Detect stuck particles
  //
  // The basic algorithm is to allow the user to specify an optional
  // threshold where we detect whenever a particle is contacting
  // more than one fixture for more than threshold consecutive
  // steps. This is considered to be "stuck", and these are put
  // in a list the user can query per step, if enabled, to deal with
  // such particles.

  if (m_stuckThreshold <= 0)
  {
    return;
  }

  // Get the state variables for this particle.
  int32 * const consecutiveCount =
      &m_consecutiveContactStepsBuffer.data[particle];
  int32 * const lastStep = &m_lastBodyContactStepBuffer.data[particle];
  int32 * const bodyCount = &m_bodyContactCountBuffer.data[particle];

  // This is only called when there is a body contact for this particle.
  ++(*bodyCount);

  // We want to only trigger detection once per step, the first time we
  // contact more than one fixture in a step for a given particle.
  if (*bodyCount == 2)
  {
    ++(*consecutiveCount);
    if (*consecutiveCount > m_stuckThreshold)
    {
      int32& newStuckParticle = m_stuckParticleBuffer.Append();
      newStuckParticle = particle;
    }
  }
  *lastStep = m_timestamp;
}

// Get the world's contact listener if any particles with the
// b2_fixtureContactListenerParticle flag are present in the system.
inline b2ContactListener* b2ParticleSystem::GetFixtureContactListener() const
{
  return (m_allParticleFlags & b2_fixtureContactListenerParticle) ?
    m_world->m_contactManager.m_contactListener : NULL;
}

// Get the world's contact filter if any particles with the
// b2_fixtureContactFilterParticle flag are present in the system.
inline b2ContactFilter* b2ParticleSystem::GetFixtureContactFilter() const
{
  return (m_allParticleFlags & b2_fixtureContactFilterParticle) ?
    m_world->m_contactManager.m_contactFilter : NULL;
}

/// Compute the axis-aligned bounding box for all particles contained
/// within this particle system.
/// @param aabb Returns the axis-aligned bounding box of the system.
void b2ParticleSystem::ComputeAABB(b2AABB* const aabb) const
{
  const int32 particleCount = GetParticleCount();
  b2Assert(aabb);
  aabb->lowerBound.x = +b2_maxFloat;
  aabb->lowerBound.y = +b2_maxFloat;
  aabb->upperBound.x = -b2_maxFloat;
  aabb->upperBound.y = -b2_maxFloat;

  for (int32 i = 0; i < particleCount; i++)
  {
    b2Vec2 p = m_positionBuffer.data[i];
    aabb->lowerBound = b2Min(aabb->lowerBound, p);
    aabb->upperBound = b2Max(aabb->upperBound, p);
  }
  aabb->lowerBound.x -= m_particleDiameter;
  aabb->lowerBound.y -= m_particleDiameter;
  aabb->upperBound.x += m_particleDiameter;
  aabb->upperBound.y += m_particleDiameter;
}

// Associate a memory allocator with this object.
FixedSetAllocator::FixedSetAllocator(
    b2StackAllocator* allocator) :
  m_buffer(NULL), m_valid(NULL), m_count(0), m_allocator(allocator)
{
  b2Assert(allocator);
}

// Allocate internal storage for this object.
int32 FixedSetAllocator::Allocate(
  const int32 itemSize, const int32 count)
{
  Clear();
  if (count)
  {
    m_buffer = m_allocator->Allocate(
      (itemSize + sizeof(*m_valid)) * count);
    b2Assert(m_buffer);
    m_valid = (int8*)m_buffer + (itemSize * count);
    memset(m_valid, 1, sizeof(*m_valid) * count);
    m_count = count;
  }
  return m_count;
}

// Deallocate the internal buffer if it's allocated.
void FixedSetAllocator::Clear()
{
  if (m_buffer)
  {
    m_allocator->Free(m_buffer);
    m_buffer = NULL;
        m_count = 0;
  }
}

// Search set for item returning the index of the item if it's found, -1
// otherwise.
template<typename T>
static int32 FindItemIndexInFixedSet(const TypedFixedSetAllocator<T>& set,
                   const T& item)
{
  if (set.GetCount())
  {
    const T* buffer = set.GetBuffer();
    const T* last = buffer + set.GetCount();
    const T* found = std::lower_bound( buffer, buffer + set.GetCount(),
                      item, T::Compare);
    if( found != last )
    {
      return set.GetIndex( found );
    }
  }
  return -1;
}

// Initialize from a set of particle / body contacts for particles
// that have the b2_fixtureContactListenerParticle flag set.
void FixtureParticleSet::Initialize(
  const b2ParticleBodyContact * const bodyContacts,
  const int32 numBodyContacts,
  const uint32 * const particleFlagsBuffer)
{
  Clear();
  if (Allocate(numBodyContacts))
  {
    FixtureParticle* set = GetBuffer();
    int32 insertedContacts = 0;
    for (int32 i = 0; i < numBodyContacts; ++i)
    {
      FixtureParticle* const fixtureParticle = &set[i];
      const b2ParticleBodyContact& bodyContact = bodyContacts[i];
      if (bodyContact.index == b2_invalidParticleIndex ||
        !(particleFlagsBuffer[bodyContact.index] &
          b2_fixtureContactListenerParticle))
      {
        continue;
      }
      fixtureParticle->first = bodyContact.fixture;
      fixtureParticle->second = bodyContact.index;
      insertedContacts++;
    }
    SetCount(insertedContacts);
    std::sort(set, set + insertedContacts, FixtureParticle::Compare);
  }
}

// Find the index of a particle / fixture pair in the set or -1 if it's not
// present.
int32 FixtureParticleSet::Find(
  const FixtureParticle& fixtureParticle) const
{
  return FindItemIndexInFixedSet(*this, fixtureParticle);
}

// Initialize from a set of particle contacts.
void b2ParticlePairSet::Initialize(
  const b2ParticleContact * const contacts, const int32 numContacts,
  const uint32 * const particleFlagsBuffer)
{
  Clear();
  if (Allocate(numContacts))
  {
    ParticlePair* set = GetBuffer();
    int32 insertedContacts = 0;
    for (int32 i = 0; i < numContacts; ++i)
    {
      ParticlePair* const pair = &set[i];
      const b2ParticleContact& contact = contacts[i];
      if (contact.GetIndexA() == b2_invalidParticleIndex ||
        contact.GetIndexB() == b2_invalidParticleIndex ||
        !((particleFlagsBuffer[contact.GetIndexA()] |
           particleFlagsBuffer[contact.GetIndexB()]) &
          b2_particleContactListenerParticle))
      {
        continue;
      }
      pair->first = contact.GetIndexA();
      pair->second = contact.GetIndexB();
      insertedContacts++;
    }
    SetCount(insertedContacts);
    std::sort(set, set + insertedContacts, ParticlePair::Compare);
  }
}

// Find the index of a particle pair in the set or -1 if it's not present.
int32 b2ParticlePairSet::Find(const ParticlePair& pair) const
{
  int32 index = FindItemIndexInFixedSet(*this, pair);
  if (index < 0)
  {
    ParticlePair swapped;
    swapped.first = pair.second;
    swapped.second = pair.first;
    index = FindItemIndexInFixedSet(*this, swapped);
  }
  return index;
}

/// Callback class to receive pairs of fixtures and particles which may be
/// overlapping. Used as an argument of b2World::QueryAABB.
class b2FixtureParticleQueryCallback : public b2QueryCallback
{
public:
  explicit b2FixtureParticleQueryCallback(b2ParticleSystem* system)
  {
    m_system = system;
  }

private:
  // Skip reporting particles.
  bool ShouldQueryParticleSystem(const b2ParticleSystem* system)
  {
    B2_NOT_USED(system);
    return false;
  }

  // Receive a fixture and call ReportFixtureAndParticle() for each particle
  // inside aabb of the fixture.
  bool ReportFixture(b2Fixture* fixture)
  {
    if (fixture->IsSensor())
    {
      return true;
    }
    const b2Shape* shape = fixture->GetShape();
    // TODO needs UpdateAABB?
    b2AABB aabb = fixture->GetAABB();
    b2ParticleSystem::InsideBoundsEnumerator enumerator =
              m_system->GetInsideBoundsEnumerator(aabb);
    int32 index;
    while ((index = enumerator.GetNext()) >= 0)
    {
      ReportFixtureAndParticle(fixture, index);
    }
    return true;
  }

  // Receive a fixture and a particle which may be overlapping.
  virtual void ReportFixtureAndParticle(
            b2Fixture* fixture, int32 index) = 0;

protected:
  b2ParticleSystem* m_system;
};

void b2ParticleSystem::NotifyBodyContactListenerPreContact(
  FixtureParticleSet* fixtureSet) const
{
  b2ContactListener* const contactListener = GetFixtureContactListener();
  if (contactListener == NULL)
    return;

  fixtureSet->Initialize(m_bodyContactBuffer.Begin(),
               m_bodyContactBuffer.GetCount(),
               GetFlagsBuffer());
}

// If a contact listener is present and the contact is just starting
// report the contact.  If the contact is already in progress invalid
// the contact from m_fixtureSet.
void b2ParticleSystem::NotifyBodyContactListenerPostContact(
  FixtureParticleSet& fixtureSet)
{
  b2ContactListener* const contactListener = GetFixtureContactListener();
  if (contactListener == NULL)
    return;

  // Loop through all new contacts, reporting any new ones, and
  // "invalidating" the ones that still exist.
  for (b2ParticleBodyContact* contact = m_bodyContactBuffer.Begin();
     contact != m_bodyContactBuffer.End(); ++contact)
  {
    b2Assert(contact);
    FixtureParticle fixtureParticleToFind;
    fixtureParticleToFind.first = contact->fixture;
    fixtureParticleToFind.second = contact->index;
    const int32 index = fixtureSet.Find(fixtureParticleToFind);
    if (index >= 0)
    {
      // Already touching remove this from the set.
      fixtureSet.Invalidate(index);
    }
    else
    {
      // Just started touching, report it!
      contactListener->BeginContact(this, contact);
    }
  }

  // If the contact listener is enabled, report all fixtures that are no
  // longer in contact with particles.
  const FixtureParticle* const fixtureParticles = fixtureSet.GetBuffer();
  const int8* const fixtureParticlesValid = fixtureSet.GetValidBuffer();
  const int32 fixtureParticleCount = fixtureSet.GetCount();
  for (int32 i = 0; i < fixtureParticleCount; ++i)
  {
    if (fixtureParticlesValid[i])
    {
      const FixtureParticle* const fixtureParticle =
        &fixtureParticles[i];
      contactListener->EndContact(fixtureParticle->first, this,
                    fixtureParticle->second);
    }
  }
}


void b2ParticleSystem::UpdateBodyContacts()
{
  // If the particle contact listener is enabled, generate a set of
  // fixture / particle contacts.
  FixtureParticleSet fixtureSet(&m_world->m_stackAllocator);
  NotifyBodyContactListenerPreContact(&fixtureSet);

  if (m_stuckThreshold > 0)
  {
    const int32 particleCount = GetParticleCount();
    for (int32 i = 0; i < particleCount; i++)
    {
      // Detect stuck particles, see comment in
      // b2ParticleSystem::DetectStuckParticle()
      m_bodyContactCountBuffer.data[i] = 0;
      if (m_timestamp > (m_lastBodyContactStepBuffer.data[i] + 1))
      {
        m_consecutiveContactStepsBuffer.data[i] = 0;
      }
    }
  }
  m_bodyContactBuffer.SetCount(0);
  m_stuckParticleBuffer.SetCount(0);

  class UpdateBodyContactsCallback : public b2FixtureParticleQueryCallback
  {
    // Call the contact filter if it's set, to determine whether to
    // filter this contact.  Returns true if contact calculations should
    // be performed, false otherwise.
    inline bool ShouldCollide(b2Fixture * const fixture,
                  int32 particleIndex)
    {
      if (m_contactFilter)
      {
        const uint32* const flags = m_system->GetFlagsBuffer();
        if (flags[particleIndex] & b2_fixtureContactFilterParticle)
        {
          return m_contactFilter->ShouldCollide(fixture, m_system,
                              particleIndex);
        }
      }
      return true;
    }

    void ReportFixtureAndParticle(
                b2Fixture* fixture, int32 a)
    {
      b2Vec2 ap = m_system->m_positionBuffer.data[a];
      float32 d;
      b2Vec2 n;
      fixture->ComputeDistance(ap, &d, &n);

      if (d < m_system->m_particleDiameter && ShouldCollide(fixture, a))
      {
        b2Body* b = fixture->GetBody();
        b2Vec2 bp = b->GetWorldCenter();
        float32 bm = b->GetMass();
        float32 bI =
          b->GetInertia() - bm * b->GetLocalCenter().LengthSquared();
        float32 invBm = bm > 0 ? 1 / bm : 0;
        float32 invBI = bI > 0 ? 1 / bI : 0;
        float32 invAm =
          m_system->m_flagsBuffer.data[a] &
          b2_wallParticle ? 0 : m_system->GetParticleInvMass();
        b2Vec2 rp = ap - bp;
        float32 rpn = b2Cross(rp, n);
        float32 invM = invAm + invBm + invBI * rpn * rpn;

        b2ParticleBodyContact& contact =
          m_system->m_bodyContactBuffer.Append();
        contact.index = a;
        contact.body = b;
        contact.fixture = fixture;
        contact.weight = 1 - d * m_system->m_inverseDiameter;
        contact.normal = -n;
        contact.mass = invM > 0 ? 1 / invM : 0;
        m_system->DetectStuckParticle(a);
      }
    }

    b2ContactFilter* m_contactFilter;

  public:
    UpdateBodyContactsCallback(
      b2ParticleSystem* system, b2ContactFilter* contactFilter):
      b2FixtureParticleQueryCallback(system)
    {
      m_contactFilter = contactFilter;
    }
  } callback(this, GetFixtureContactFilter());

  b2AABB aabb;
  ComputeAABB(&aabb);
  m_world->QueryAABB(&callback, aabb);

  if (m_def.strictContactCheck)
  {
    RemoveSpuriousBodyContacts();
  }

  NotifyBodyContactListenerPostContact(fixtureSet);
}

void b2ParticleSystem::RemoveSpuriousBodyContacts()
{
  // At this point we have a list of contact candidates based on AABB
  // overlap.The AABB query that  generated this returns all collidable
  // fixtures overlapping particle bounding boxes.  This breaks down around
  // vertices where two shapes intersect, such as a "ground" surface made
  // of multiple b2PolygonShapes; it potentially applies a lot of spurious
  // impulses from normals that should not actually contribute.  See the
  // Ramp example in Testbed.
  //
  // To correct for this, we apply this algorithm:
  //   * sort contacts by particle and subsort by weight (nearest to farthest)
  //   * for each contact per particle:
  //      - project a point at the contact distance along the inverse of the
  //        contact normal
  //      - if this intersects the fixture that generated the contact, apply
  //         it, otherwise discard as impossible
  //      - repeat for up to n nearest contacts, currently we get good results
  //        from n=3.
  std::sort(m_bodyContactBuffer.Begin(), m_bodyContactBuffer.End(),
        b2ParticleSystem::BodyContactCompare);

  int32 discarded = 0;
  (void) std::remove_if(m_bodyContactBuffer.Begin(),
          m_bodyContactBuffer.End(),
          b2ParticleBodyContactRemovePredicate(this, &discarded));

  m_bodyContactBuffer.SetCount(m_bodyContactBuffer.GetCount() - discarded);
}

bool b2ParticleSystem::BodyContactCompare(const b2ParticleBodyContact &lhs,
                      const b2ParticleBodyContact &rhs)
{
  if (lhs.index == rhs.index)
  {
    // Subsort by weight, decreasing.
    return lhs.weight > rhs.weight;
  }
  return lhs.index < rhs.index;
}


void b2ParticleSystem::SolveCollision(const b2TimeStep& step)
{
  // This function detects particles which are crossing boundary of bodies
  // and modifies velocities of them so that they will move just in front of
  // boundary. This function function also applies the reaction force to
  // bodies as precisely as the numerical stability is kept.
  b2AABB aabb;
  aabb.lowerBound.x = +b2_maxFloat;
  aabb.lowerBound.y = +b2_maxFloat;
  aabb.upperBound.x = -b2_maxFloat;
  aabb.upperBound.y = -b2_maxFloat;
  for (int32 i = 0; i < m_count; i++)
  {
    b2Vec2 v = m_velocityBuffer.data[i];
    b2Vec2 p1 = m_positionBuffer.data[i];
    b2Vec2 p2 = p1 + step.dt * v;
    aabb.lowerBound = b2Min(aabb.lowerBound, b2Min(p1, p2));
    aabb.upperBound = b2Max(aabb.upperBound, b2Max(p1, p2));
  }
  class SolveCollisionCallback : public b2FixtureParticleQueryCallback
  {
    // Call the contact filter if it's set, to determine whether to
    // filter this contact.  Returns true if contact calculations should
    // be performed, false otherwise.
    inline bool ShouldCollide(b2Fixture * const fixture,
                  int32 particleIndex)
    {
      if (m_contactFilter) {
        const uint32* const flags = m_system->GetFlagsBuffer();
        if (flags[particleIndex] & b2_fixtureContactFilterParticle) {
          return m_contactFilter->ShouldCollide(fixture, m_system,
                              particleIndex);
        }
      }
      return true;
    }

    void ReportFixtureAndParticle(b2Fixture* fixture, int32 a)
    {
      if (ShouldCollide(fixture, a)) {
        b2Body* body = fixture->GetBody();
        b2Vec2 ap = m_system->m_positionBuffer.data[a];
        b2Vec2 av = m_system->m_velocityBuffer.data[a];
        b2RayCastOutput output;
        b2RayCastInput input;
        if (m_system->m_iterationIndex == 0)
        {
          // Put 'ap' in the local space of the previous frame

          b2Vec2 p1 = b2MulT(body->m_xf0, ap);

          if (fixture->GetShape()->GetType() == b2Shape::e_circle)
          {
            // Make relative to the center of the circle
            p1 -= body->GetLocalCenter();
            // Re-apply rotation about the center of the
            // circle

            p1 = b2Mul(body->m_xf0.q, p1);

            // Subtract rotation of the current frame
            p1 = b2MulT(body->m_xf.q, p1);
            // Return to local space
            p1 += body->GetLocalCenter();
          }
          // Return to global space and apply rotation of current frame
          input.p1 = b2Mul(body->m_xf, p1);
        }
        else
        {
          input.p1 = ap;
        }
        input.p2 = ap + m_step.dt * av;
        input.maxFraction = 1;
        if (fixture->RayCast(&output, input))
        {
          float32 frac = output.fraction;
          b2Vec2 n = output.normal;
          b2Vec2 p = (1 - frac) * input.p1 + frac * input.p2 + b2_linearSlop * n;
          b2Vec2 v = m_step.inv_dt * (p - ap);
          m_system->m_velocityBuffer.data[a] = v;
          b2Vec2 f = m_step.inv_dt * m_system->GetParticleMass() * (av - v);
          m_system->ParticleApplyForce(a, f);
        }
      }
    }

    b2TimeStep m_step;
    b2ContactFilter* m_contactFilter;

  public:
    SolveCollisionCallback(
      b2ParticleSystem* system, const b2TimeStep& step, b2ContactFilter* contactFilter) :
      b2FixtureParticleQueryCallback(system)
    {
      m_step = step;
      m_contactFilter = contactFilter;
    }
  } callback(this, step, GetFixtureContactFilter());
  m_world->QueryAABB(&callback, aabb);
}

void b2ParticleSystem::SolveBarrier(const b2TimeStep& step)
{
  // If a particle is passing between paired barrier particles,
  // its velocity will be decelerated to avoid passing.
  for (int32 i = 0; i < m_count; i++)
  {
    uint32 flags = m_flagsBuffer.data[i];
    static const uint32 k_barrierWallFlags =
                    b2_barrierParticle | b2_wallParticle;
    if ((flags & k_barrierWallFlags) == k_barrierWallFlags)
    {
      m_velocityBuffer.data[i].SetZero();
    }
  }
  float32 tmax = b2_barrierCollisionTime * step.dt;
  for (int32 k = 0; k < m_pairBuffer.GetCount(); k++)
  {
    const b2ParticlePair& pair = m_pairBuffer[k];
    if (pair.flags & b2_barrierParticle)
    {
      int32 a = pair.indexA;
      int32 b = pair.indexB;
      b2Vec2 pa = m_positionBuffer.data[a];
      b2Vec2 pb = m_positionBuffer.data[b];
      b2AABB aabb;
      aabb.lowerBound = b2Min(pa, pb);
      aabb.upperBound = b2Max(pa, pb);
      b2ParticleGroup *aGroup = m_groupBuffer[a];
      b2ParticleGroup *bGroup = m_groupBuffer[b];
      b2Vec2 va = GetLinearVelocity(aGroup, a, pa);
      b2Vec2 vb = GetLinearVelocity(bGroup, b, pb);
      b2Vec2 pba = pb - pa;
      b2Vec2 vba = vb - va;
      InsideBoundsEnumerator enumerator = GetInsideBoundsEnumerator(aabb);
      int32 c;
      while ((c = enumerator.GetNext()) >= 0)
      {
        b2Vec2 pc = m_positionBuffer.data[c];
        b2ParticleGroup *cGroup = m_groupBuffer[c];
        if (aGroup != cGroup && bGroup != cGroup)
        {
          b2Vec2 vc = GetLinearVelocity(cGroup, c, pc);
          // Solve the equation below:
          //   (1-s)*(pa+t*va)+s*(pb+t*vb) = pc+t*vc
          // which expresses that the particle c will pass a line
          // connecting the particles a and b at the time of t.
          // if s is between 0 and 1, c will pass between a and b.
          b2Vec2 pca = pc - pa;
          b2Vec2 vca = vc - va;
          float32 e2 = b2Cross(vba, vca);
          float32 e1 = b2Cross(pba, vca) - b2Cross(pca, vba);
          float32 e0 = b2Cross(pba, pca);
          float32 s, t;
          b2Vec2 qba, qca;
          if (e2 == 0)
          {
            if (e1 == 0) continue;
            t = - e0 / e1;
            if (!(t >= 0 && t < tmax)) continue;
            qba = pba + t * vba;
            qca = pca + t * vca;
            s = b2Dot(qba, qca) / b2Dot(qba, qba);
            if (!(s >= 0 && s <= 1)) continue;
          }
          else
          {
            float32 det = e1 * e1 - 4 * e0 * e2;
            if (det < 0) continue;
            float32 sqrtDet = b2Sqrt(det);
            float32 t1 = (- e1 - sqrtDet) / (2 * e2);
            float32 t2 = (- e1 + sqrtDet) / (2 * e2);
            if (t1 > t2) b2Swap(t1, t2);
            t = t1;
            qba = pba + t * vba;
            qca = pca + t * vca;
            s = b2Dot(qba, qca) / b2Dot(qba, qba);
            if (!(t >= 0 && t < tmax && s >= 0 && s <= 1))
            {
              t = t2;
              if (!(t >= 0 && t < tmax)) continue;
              qba = pba + t * vba;
              qca = pca + t * vca;
              s = b2Dot(qba, qca) / b2Dot(qba, qba);
              if (!(s >= 0 && s <= 1)) continue;
            }
          }
          // Apply a force to particle c so that it will have the
          // interpolated velocity at the collision point on line ab.
          b2Vec2 dv = va + s * vba - vc;
          b2Vec2 f = GetParticleMass() * dv;
          if (IsRigidGroup(cGroup))
          {
            // If c belongs to a rigid group, the force will be
            // distributed in the group.
            float32 mass = cGroup->GetMass();
            float32 inertia = cGroup->GetInertia();
            if (mass > 0)
            {
              cGroup->m_linearVelocity += 1 / mass * f;
            }
            if (inertia > 0)
            {
              cGroup->m_angularVelocity +=
                b2Cross(pc - cGroup->GetCenter(), f) / inertia;
            }
          }
          else
          {
            m_velocityBuffer.data[c] += dv;
          }
          // Apply a reversed force to particle c after particle
          // movement so that momentum will be preserved.
          ParticleApplyForce(c, -step.inv_dt * f);
        }
      }
    }
  }
}

void b2ParticleSystem::Solve(const b2TimeStep& step)
{
  if (m_count == 0)
  {
    return;
  }
  // If particle lifetimes are enabled, destroy particles that are too old.
  if (m_expirationTimeBuffer.data)
  {
    SolveLifetimes(step);
  }
  if (m_allParticleFlags & b2_zombieParticle)
  {
    SolveZombie();
  }
  if (m_needsUpdateAllParticleFlags)
  {
    UpdateAllParticleFlags();
  }
  if (m_needsUpdateAllGroupFlags)
  {
    UpdateAllGroupFlags();
  }
  if (m_paused)
  {
    return;
  }
  for (m_iterationIndex = 0;
    m_iterationIndex < step.particleIterations;
    m_iterationIndex++)
  {
    ++m_timestamp;
    b2TimeStep subStep = step;
    subStep.dt /= step.particleIterations;
    subStep.inv_dt *= step.particleIterations;
    UpdateContacts(false);
    UpdateBodyContacts();
    ComputeWeight();
    if (m_allGroupFlags & b2_particleGroupNeedsUpdateDepth)
    {
      ComputeDepth();
    }
    if (m_allParticleFlags & b2_reactiveParticle)
    {
      UpdatePairsAndTriadsWithReactiveParticles();
    }
    if (m_hasForce)
    {
      SolveForce(subStep);
    }
    if (m_allParticleFlags & b2_viscousParticle)
    {
      SolveViscous();
    }
    if (m_allParticleFlags & b2_repulsiveParticle)
    {
      SolveRepulsive(subStep);
    }
    if (m_allParticleFlags & b2_powderParticle)
    {
      SolvePowder(subStep);
    }
    if (m_allParticleFlags & b2_tensileParticle)
    {
      SolveTensile(subStep);
    }
    if (m_allGroupFlags & b2_solidParticleGroup)
    {
      SolveSolid(subStep);
    }
    if (m_allParticleFlags & b2_colorMixingParticle)
    {
      SolveColorMixing();
    }
    SolveGravity(subStep);
    if (m_allParticleFlags & b2_staticPressureParticle)
    {
      SolveStaticPressure(subStep);
    }
    SolvePressure(subStep);
    SolveDamping(subStep);
    if (m_allParticleFlags & k_extraDampingFlags)
    {
      SolveExtraDamping();
    }
    // SolveElastic and SolveSpring refer the current velocities for
    // numerical stability, they should be called as late as possible.
    if (m_allParticleFlags & b2_elasticParticle)
    {
      SolveElastic(subStep);
    }
    if (m_allParticleFlags & b2_springParticle)
    {
      SolveSpring(subStep);
    }
    LimitVelocity(subStep);
    if (m_allGroupFlags & b2_rigidParticleGroup)
    {
      SolveRigidDamping();
    }
    if (m_allParticleFlags & b2_barrierParticle)
    {
      SolveBarrier(subStep);
    }
    // SolveCollision, SolveRigid and SolveWall should be called after
    // other force functions because they may require particles to have
    // specific velocities.
    SolveCollision(subStep);
    if (m_allGroupFlags & b2_rigidParticleGroup)
    {
      SolveRigid(subStep);
    }
    if (m_allParticleFlags & b2_wallParticle)
    {
      SolveWall();
    }
    // The particle positions can be updated only at the end of substep.
    for (int32 i = 0; i < m_count; i++)
    {
      m_positionBuffer.data[i] += subStep.dt * m_velocityBuffer.data[i];
    }
  }
}

void b2ParticleSystem::UpdateAllParticleFlags()
{
  m_allParticleFlags = 0;
  for (int32 i = 0; i < m_count; i++)
  {
    m_allParticleFlags |= m_flagsBuffer.data[i];
  }
  m_needsUpdateAllParticleFlags = false;
}

void b2ParticleSystem::UpdateAllGroupFlags()
{
  m_allGroupFlags = 0;
  for (const b2ParticleGroup* group = m_groupList; group;
     group = group->GetNext())
  {
    m_allGroupFlags |= group->m_groupFlags;
  }
  m_needsUpdateAllGroupFlags = false;
}

void b2ParticleSystem::LimitVelocity(const b2TimeStep& step)
{
  float32 criticalVelocitySquared = GetCriticalVelocitySquared(step);
  for (int32 i = 0; i < m_count; i++)
  {
    b2Vec2& v = m_velocityBuffer.data[i];
    float32 v2 = b2Dot(v, v);
    if (v2 > criticalVelocitySquared)
    {
      v *= b2Sqrt(criticalVelocitySquared / v2);
    }
  }
}

void b2ParticleSystem::SolveGravity(const b2TimeStep& step)
{
  b2Vec2 gravity = step.dt * m_def.gravityScale * m_world->GetGravity();
  for (int32 i = 0; i < m_count; i++)
  {
    m_velocityBuffer.data[i] += gravity;
  }
}

void b2ParticleSystem::SolveStaticPressure(const b2TimeStep& step)
{
  m_staticPressureBuffer = RequestBuffer(m_staticPressureBuffer);
  float32 criticalPressure = GetCriticalPressure(step);
  float32 pressurePerWeight = m_def.staticPressureStrength * criticalPressure;
  float32 maxPressure = b2_maxParticlePressure * criticalPressure;
  float32 relaxation = m_def.staticPressureRelaxation;
  /// Compute pressure satisfying the modified Poisson equation:
  ///     Sum_for_j((p_i - p_j) * w_ij) + relaxation * p_i =
  ///     pressurePerWeight * (w_i - b2_minParticleWeight)
  /// by iterating the calculation:
  ///     p_i = (Sum_for_j(p_j * w_ij) + pressurePerWeight *
  ///           (w_i - b2_minParticleWeight)) / (w_i + relaxation)
  /// where
  ///     p_i and p_j are static pressure of particle i and j
  ///     w_ij is contact weight between particle i and j
  ///     w_i is sum of contact weight of particle i
  for (int32 t = 0; t < m_def.staticPressureIterations; t++)
  {
    memset(m_accumulationBuffer, 0,
         sizeof(*m_accumulationBuffer) * m_count);
    for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
    {
      const b2ParticleContact& contact = m_contactBuffer[k];
      if (contact.GetFlags() & b2_staticPressureParticle)
      {
        int32 a = contact.GetIndexA();
        int32 b = contact.GetIndexB();
        float32 w = contact.GetWeight();
        m_accumulationBuffer[a] +=
          w * m_staticPressureBuffer[b]; // a <- b
        m_accumulationBuffer[b] +=
          w * m_staticPressureBuffer[a]; // b <- a
      }
    }
    for (int32 i = 0; i < m_count; i++)
    {
      float32 w = m_weightBuffer[i];
      if (m_flagsBuffer.data[i] & b2_staticPressureParticle)
      {
        float32 wh = m_accumulationBuffer[i];
        float32 h =
          (wh + pressurePerWeight * (w - b2_minParticleWeight)) /
          (w + relaxation);
        m_staticPressureBuffer[i] = b2Clamp(h, 0.0f, maxPressure);
      }
      else
      {
        m_staticPressureBuffer[i] = 0;
      }
    }
  }
}

void b2ParticleSystem::SolvePressure(const b2TimeStep& step)
{
  // calculates pressure as a linear function of density
  float32 criticalPressure = GetCriticalPressure(step);
  float32 pressurePerWeight = m_def.pressureStrength * criticalPressure;
  float32 maxPressure = b2_maxParticlePressure * criticalPressure;
  for (int32 i = 0; i < m_count; i++)
  {
    float32 w = m_weightBuffer[i];
    float32 h = pressurePerWeight * b2Max(0.0f, w - b2_minParticleWeight);
    m_accumulationBuffer[i] = b2Min(h, maxPressure);
  }
  // ignores particles which have their own repulsive force
  if (m_allParticleFlags & k_noPressureFlags)
  {
    for (int32 i = 0; i < m_count; i++)
    {
      if (m_flagsBuffer.data[i] & k_noPressureFlags)
      {
        m_accumulationBuffer[i] = 0;
      }
    }
  }
  // static pressure
  if (m_allParticleFlags & b2_staticPressureParticle)
  {
    b2Assert(m_staticPressureBuffer);
    for (int32 i = 0; i < m_count; i++)
    {
      if (m_flagsBuffer.data[i] & b2_staticPressureParticle)
      {
        m_accumulationBuffer[i] += m_staticPressureBuffer[i];
      }
    }
  }
  // applies pressure between each particles in contact
  float32 velocityPerPressure = step.dt / (m_def.density * m_particleDiameter);
  for (int32 k = 0; k < m_bodyContactBuffer.GetCount(); k++)
  {
    const b2ParticleBodyContact& contact = m_bodyContactBuffer[k];
    int32 a = contact.index;
    b2Body* b = contact.body;
    float32 w = contact.weight;
    float32 m = contact.mass;
    b2Vec2 n = contact.normal;
    b2Vec2 p = m_positionBuffer.data[a];
    float32 h = m_accumulationBuffer[a] + pressurePerWeight * w;
    b2Vec2 f = velocityPerPressure * w * m * h * n;
    m_velocityBuffer.data[a] -= GetParticleInvMass() * f;
    b->ApplyLinearImpulse(f, p);
  }
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    int32 a = contact.GetIndexA();
    int32 b = contact.GetIndexB();
    float32 w = contact.GetWeight();
    b2Vec2 n = contact.GetNormal();
    float32 h = m_accumulationBuffer[a] + m_accumulationBuffer[b];
    b2Vec2 f = velocityPerPressure * w * h * n;
    m_velocityBuffer.data[a] -= f;
    m_velocityBuffer.data[b] += f;
  }
}

void b2ParticleSystem::SolveDamping(const b2TimeStep& step)
{
  // reduces normal velocity of each contact
  float32 linearDamping = m_def.dampingStrength;
  float32 quadraticDamping = 1 / GetCriticalVelocity(step);
  for (int32 k = 0; k < m_bodyContactBuffer.GetCount(); k++)
  {
    const b2ParticleBodyContact& contact = m_bodyContactBuffer[k];
    int32 a = contact.index;
    b2Body* b = contact.body;
    float32 w = contact.weight;
    float32 m = contact.mass;
    b2Vec2 n = contact.normal;
    b2Vec2 p = m_positionBuffer.data[a];
    b2Vec2 v = b->GetLinearVelocityFromWorldPoint(p) -
           m_velocityBuffer.data[a];
    float32 vn = b2Dot(v, n);
    if (vn < 0)
    {
      float32 damping =
        b2Max(linearDamping * w, b2Min(- quadraticDamping * vn, 0.5f));
      b2Vec2 f = damping * m * vn * n;
      m_velocityBuffer.data[a] += GetParticleInvMass() * f;
      b->ApplyLinearImpulse(-f, p);
    }
  }
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    int32 a = contact.GetIndexA();
    int32 b = contact.GetIndexB();
    float32 w = contact.GetWeight();
    b2Vec2 n = contact.GetNormal();
    b2Vec2 v = m_velocityBuffer.data[b] - m_velocityBuffer.data[a];
    float32 vn = b2Dot(v, n);
    if (vn < 0)
    {
      float32 damping =
        b2Max(linearDamping * w, b2Min(- quadraticDamping * vn, 0.5f));
      b2Vec2 f = damping * vn * n;
      m_velocityBuffer.data[a] += f;
      m_velocityBuffer.data[b] -= f;
    }
  }
}

inline bool b2ParticleSystem::IsRigidGroup(b2ParticleGroup *group) const
{
  return group && (group->m_groupFlags & b2_rigidParticleGroup);
}

inline b2Vec2 b2ParticleSystem::GetLinearVelocity(
  b2ParticleGroup *group, int32 particleIndex,
  const b2Vec2 &point) const
{
  if (IsRigidGroup(group))
  {
    return group->GetLinearVelocityFromWorldPoint(point);
  }
  else
  {
    return m_velocityBuffer.data[particleIndex];
  }
}

inline void b2ParticleSystem::InitDampingParameter(
  float32* invMass, float32* invInertia, float32* tangentDistance,
  float32 mass, float32 inertia, const b2Vec2& center,
  const b2Vec2& point, const b2Vec2& normal) const
{
  *invMass = mass > 0 ? 1 / mass : 0;
  *invInertia = inertia > 0 ? 1 / inertia : 0;
  *tangentDistance = b2Cross(point - center, normal);
}

inline void b2ParticleSystem::InitDampingParameterWithRigidGroupOrParticle(
  float32* invMass, float32* invInertia, float32* tangentDistance,
  bool isRigidGroup, b2ParticleGroup* group, int32 particleIndex,
  const b2Vec2& point, const b2Vec2& normal) const
{
  if (isRigidGroup)
  {
    InitDampingParameter(
      invMass, invInertia, tangentDistance,
      group->GetMass(), group->GetInertia(), group->GetCenter(),
      point, normal);
  }
  else
  {
    uint32 flags = m_flagsBuffer.data[particleIndex];
    InitDampingParameter(
      invMass, invInertia, tangentDistance,
      flags & b2_wallParticle ? 0 : GetParticleMass(), 0, point,
      point, normal);
  }
}

inline float32 b2ParticleSystem::ComputeDampingImpulse(
  float32 invMassA, float32 invInertiaA, float32 tangentDistanceA,
  float32 invMassB, float32 invInertiaB, float32 tangentDistanceB,
  float32 normalVelocity) const
{
  float32 invMass =
    invMassA + invInertiaA * tangentDistanceA * tangentDistanceA +
    invMassB + invInertiaB * tangentDistanceB * tangentDistanceB;
  return invMass > 0 ? normalVelocity / invMass : 0;
}

inline void b2ParticleSystem::ApplyDamping(
  float32 invMass, float32 invInertia, float32 tangentDistance,
  bool isRigidGroup, b2ParticleGroup* group, int32 particleIndex,
  float32 impulse, const b2Vec2& normal)
{
  if (isRigidGroup)
  {
    group->m_linearVelocity += impulse * invMass * normal;
    group->m_angularVelocity += impulse * tangentDistance * invInertia;
  }
  else
  {
    m_velocityBuffer.data[particleIndex] += impulse * invMass * normal;
  }
}

void b2ParticleSystem::SolveRigidDamping()
{
  // Apply impulse to rigid particle groups colliding with other objects
  // to reduce relative velocity at the colliding point.
  float32 damping = m_def.dampingStrength;
  for (int32 k = 0; k < m_bodyContactBuffer.GetCount(); k++)
  {
    const b2ParticleBodyContact& contact = m_bodyContactBuffer[k];
    int32 a = contact.index;
    b2ParticleGroup* aGroup = m_groupBuffer[a];
    if (IsRigidGroup(aGroup))
    {
      b2Body* b = contact.body;
      b2Vec2 n = contact.normal;
      float32 w = contact.weight;
      b2Vec2 p = m_positionBuffer.data[a];
      b2Vec2 v = b->GetLinearVelocityFromWorldPoint(p) -
             aGroup->GetLinearVelocityFromWorldPoint(p);
      float32 vn = b2Dot(v, n);
      if (vn < 0)
      // The group's average velocity at particle position 'p' is pushing
      // the particle into the body.
      {
        float32 invMassA, invInertiaA, tangentDistanceA;
        float32 invMassB, invInertiaB, tangentDistanceB;
        InitDampingParameterWithRigidGroupOrParticle(
          &invMassA, &invInertiaA, &tangentDistanceA,
          true, aGroup, a, p, n);
        InitDampingParameter(
          &invMassB, &invInertiaB, &tangentDistanceB,
          b->GetMass(),
          // Calculate b->m_I from public functions of b2Body.
          b->GetInertia() -
              b->GetMass() * b->GetLocalCenter().LengthSquared(),
          b->GetWorldCenter(),
          p, n);
        float32 f = damping * b2Min(w, 1.0f) * ComputeDampingImpulse(
          invMassA, invInertiaA, tangentDistanceA,
          invMassB, invInertiaB, tangentDistanceB,
          vn);
        ApplyDamping(
          invMassA, invInertiaA, tangentDistanceA,
          true, aGroup, a, f, n);
        b->ApplyLinearImpulse(-f * n, p);
      }
    }
  }
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    int32 a = contact.GetIndexA();
    int32 b = contact.GetIndexB();
    b2Vec2 n = contact.GetNormal();
    float32 w = contact.GetWeight();
    b2ParticleGroup* aGroup = m_groupBuffer[a];
    b2ParticleGroup* bGroup = m_groupBuffer[b];
    bool aRigid = IsRigidGroup(aGroup);
    bool bRigid = IsRigidGroup(bGroup);
    if (aGroup != bGroup && (aRigid || bRigid))
    {
      b2Vec2 p =
        0.5f * (m_positionBuffer.data[a] + m_positionBuffer.data[b]);
      b2Vec2 v =
        GetLinearVelocity(bGroup, b, p) -
        GetLinearVelocity(aGroup, a, p);
      float32 vn = b2Dot(v, n);
      if (vn < 0)
      {
        float32 invMassA, invInertiaA, tangentDistanceA;
        float32 invMassB, invInertiaB, tangentDistanceB;
        InitDampingParameterWithRigidGroupOrParticle(
          &invMassA, &invInertiaA, &tangentDistanceA,
          aRigid, aGroup, a,
          p, n);
        InitDampingParameterWithRigidGroupOrParticle(
          &invMassB, &invInertiaB, &tangentDistanceB,
          bRigid, bGroup, b,
          p, n);
        float32 f = damping * w * ComputeDampingImpulse(
          invMassA, invInertiaA, tangentDistanceA,
          invMassB, invInertiaB, tangentDistanceB,
          vn);
        ApplyDamping(
          invMassA, invInertiaA, tangentDistanceA,
          aRigid, aGroup, a, f, n);
        ApplyDamping(
          invMassB, invInertiaB, tangentDistanceB,
          bRigid, bGroup, b, -f, n);
      }
    }
  }
}

void b2ParticleSystem::SolveExtraDamping()
{
  // Applies additional damping force between bodies and particles which can
  // produce strong repulsive force. Applying damping force multiple times
  // is effective in suppressing vibration.
  for (int32 k = 0; k < m_bodyContactBuffer.GetCount(); k++)
  {
    const b2ParticleBodyContact& contact = m_bodyContactBuffer[k];
    int32 a = contact.index;
    if (m_flagsBuffer.data[a] & k_extraDampingFlags)
    {
      b2Body* b = contact.body;
      float32 m = contact.mass;
      b2Vec2 n = contact.normal;
      b2Vec2 p = m_positionBuffer.data[a];
      b2Vec2 v =
        b->GetLinearVelocityFromWorldPoint(p) -
        m_velocityBuffer.data[a];
      float32 vn = b2Dot(v, n);
      if (vn < 0)
      {
        b2Vec2 f = 0.5f * m * vn * n;
        m_velocityBuffer.data[a] += GetParticleInvMass() * f;
        b->ApplyLinearImpulse(-f, p);
      }
    }
  }
}

void b2ParticleSystem::SolveWall()
{
  for (int32 i = 0; i < m_count; i++)
  {
    if (m_flagsBuffer.data[i] & b2_wallParticle)
    {
      m_velocityBuffer.data[i].SetZero();
    }
  }
}

void b2ParticleSystem::SolveRigid(const b2TimeStep& step)
{
  for (b2ParticleGroup* group = m_groupList; group; group = group->GetNext())
  {
    if (group->m_groupFlags & b2_rigidParticleGroup)
    {
      group->UpdateStatistics();
      b2Rot rotation(step.dt * group->m_angularVelocity);
      b2Transform transform(
        group->m_center + step.dt * group->m_linearVelocity -
        b2Mul(rotation, group->m_center), rotation);
      group->m_transform = b2Mul(transform, group->m_transform);
      b2Transform velocityTransform;
      velocityTransform.p.x = step.inv_dt * transform.p.x;
      velocityTransform.p.y = step.inv_dt * transform.p.y;
      velocityTransform.q.s = step.inv_dt * transform.q.s;
      velocityTransform.q.c = step.inv_dt * (transform.q.c - 1);
      for (int32 i = group->m_firstIndex; i < group->m_lastIndex; i++)
      {
        m_velocityBuffer.data[i] = b2Mul(velocityTransform,
                         m_positionBuffer.data[i]);
      }
    }
  }
}

void b2ParticleSystem::SolveElastic(const b2TimeStep& step)
{
  float32 elasticStrength = step.inv_dt * m_def.elasticStrength;
  for (int32 k = 0; k < m_triadBuffer.GetCount(); k++)
  {
    const b2ParticleTriad& triad = m_triadBuffer[k];
    if (triad.flags & b2_elasticParticle)
    {
      int32 a = triad.indexA;
      int32 b = triad.indexB;
      int32 c = triad.indexC;
      const b2Vec2& oa = triad.pa;
      const b2Vec2& ob = triad.pb;
      const b2Vec2& oc = triad.pc;
      b2Vec2 pa = m_positionBuffer.data[a];
      b2Vec2 pb = m_positionBuffer.data[b];
      b2Vec2 pc = m_positionBuffer.data[c];
      b2Vec2& va = m_velocityBuffer.data[a];
      b2Vec2& vb = m_velocityBuffer.data[b];
      b2Vec2& vc = m_velocityBuffer.data[c];
      pa += step.dt * va;
      pb += step.dt * vb;
      pc += step.dt * vc;
      b2Vec2 midPoint = (float32) 1 / 3 * (pa + pb + pc);
      pa -= midPoint;
      pb -= midPoint;
      pc -= midPoint;
      b2Rot r;
      r.s = b2Cross(oa, pa) + b2Cross(ob, pb) + b2Cross(oc, pc);
      r.c = b2Dot(oa, pa) + b2Dot(ob, pb) + b2Dot(oc, pc);
      float32 r2 = r.s * r.s + r.c * r.c;
      float32 invR = b2InvSqrt(r2);
      r.s *= invR;
      r.c *= invR;
      float32 strength = elasticStrength * triad.strength;
      va += strength * (b2Mul(r, oa) - pa);
      vb += strength * (b2Mul(r, ob) - pb);
      vc += strength * (b2Mul(r, oc) - pc);
    }
  }
}

void b2ParticleSystem::SolveSpring(const b2TimeStep& step)
{
  float32 springStrength = step.inv_dt * m_def.springStrength;
  for (int32 k = 0; k < m_pairBuffer.GetCount(); k++)
  {
    const b2ParticlePair& pair = m_pairBuffer[k];
    if (pair.flags & b2_springParticle)
    {
      int32 a = pair.indexA;
      int32 b = pair.indexB;
      b2Vec2 pa = m_positionBuffer.data[a];
      b2Vec2 pb = m_positionBuffer.data[b];
      b2Vec2& va = m_velocityBuffer.data[a];
      b2Vec2& vb = m_velocityBuffer.data[b];
      pa += step.dt * va;
      pb += step.dt * vb;
      b2Vec2 d = pb - pa;
      float32 r0 = pair.distance;
      float32 r1 = d.Length();
      float32 strength = springStrength * pair.strength;
      b2Vec2 f = strength * (r0 - r1) / r1 * d;
      va -= f;
      vb += f;
    }
  }
}

void b2ParticleSystem::SolveTensile(const b2TimeStep& step)
{
  b2Assert(m_accumulation2Buffer);
  for (int32 i = 0; i < m_count; i++)
  {
    m_accumulation2Buffer[i] = b2Vec2_zero;
  }
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    if (contact.GetFlags() & b2_tensileParticle)
    {
      int32 a = contact.GetIndexA();
      int32 b = contact.GetIndexB();
      float32 w = contact.GetWeight();
      b2Vec2 n = contact.GetNormal();
      b2Vec2 weightedNormal = (1 - w) * w * n;
      m_accumulation2Buffer[a] -= weightedNormal;
      m_accumulation2Buffer[b] += weightedNormal;
    }
  }
  float32 criticalVelocity = GetCriticalVelocity(step);
  float32 pressureStrength = m_def.surfaceTensionPressureStrength
               * criticalVelocity;
  float32 normalStrength = m_def.surfaceTensionNormalStrength
               * criticalVelocity;
  float32 maxVelocityVariation = b2_maxParticleForce * criticalVelocity;
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    if (contact.GetFlags() & b2_tensileParticle)
    {
      int32 a = contact.GetIndexA();
      int32 b = contact.GetIndexB();
      float32 w = contact.GetWeight();
      b2Vec2 n = contact.GetNormal();
      float32 h = m_weightBuffer[a] + m_weightBuffer[b];
      b2Vec2 s = m_accumulation2Buffer[b] - m_accumulation2Buffer[a];
      float32 fn = b2Min(
          pressureStrength * (h - 2) + normalStrength * b2Dot(s, n),
          maxVelocityVariation) * w;
      b2Vec2 f = fn * n;
      m_velocityBuffer.data[a] -= f;
      m_velocityBuffer.data[b] += f;
    }
  }
}

void b2ParticleSystem::SolveViscous()
{
  float32 viscousStrength = m_def.viscousStrength;
  for (int32 k = 0; k < m_bodyContactBuffer.GetCount(); k++)
  {
    const b2ParticleBodyContact& contact = m_bodyContactBuffer[k];
    int32 a = contact.index;
    if (m_flagsBuffer.data[a] & b2_viscousParticle)
    {
      b2Body* b = contact.body;
      float32 w = contact.weight;
      float32 m = contact.mass;
      b2Vec2 p = m_positionBuffer.data[a];
      b2Vec2 v = b->GetLinearVelocityFromWorldPoint(p) -
             m_velocityBuffer.data[a];
      b2Vec2 f = viscousStrength * m * w * v;
      m_velocityBuffer.data[a] += GetParticleInvMass() * f;
      b->ApplyLinearImpulse(-f, p);
    }
  }
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    if (contact.GetFlags() & b2_viscousParticle)
    {
      int32 a = contact.GetIndexA();
      int32 b = contact.GetIndexB();
      float32 w = contact.GetWeight();
      b2Vec2 v = m_velocityBuffer.data[b] - m_velocityBuffer.data[a];
      b2Vec2 f = viscousStrength * w * v;
      m_velocityBuffer.data[a] += f;
      m_velocityBuffer.data[b] -= f;
    }
  }
}

void b2ParticleSystem::SolveRepulsive(const b2TimeStep& step)
{
  float32 repulsiveStrength =
    m_def.repulsiveStrength * GetCriticalVelocity(step);
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    if (contact.GetFlags() & b2_repulsiveParticle)
    {
      int32 a = contact.GetIndexA();
      int32 b = contact.GetIndexB();
      if (m_groupBuffer[a] != m_groupBuffer[b])
      {
        float32 w = contact.GetWeight();
        b2Vec2 n = contact.GetNormal();
        b2Vec2 f = repulsiveStrength * w * n;
        m_velocityBuffer.data[a] -= f;
        m_velocityBuffer.data[b] += f;
      }
    }
  }
}

void b2ParticleSystem::SolvePowder(const b2TimeStep& step)
{
  float32 powderStrength = m_def.powderStrength * GetCriticalVelocity(step);
  float32 minWeight = 1.0f - b2_particleStride;
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    if (contact.GetFlags() & b2_powderParticle)
    {
      float32 w = contact.GetWeight();
      if (w > minWeight)
      {
        int32 a = contact.GetIndexA();
        int32 b = contact.GetIndexB();
        b2Vec2 n = contact.GetNormal();
        b2Vec2 f = powderStrength * (w - minWeight) * n;
        m_velocityBuffer.data[a] -= f;
        m_velocityBuffer.data[b] += f;
      }
    }
  }
}

void b2ParticleSystem::SolveSolid(const b2TimeStep& step)
{
  // applies extra repulsive force from solid particle groups
  b2Assert(m_depthBuffer);
  float32 ejectionStrength = step.inv_dt * m_def.ejectionStrength;
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    int32 a = contact.GetIndexA();
    int32 b = contact.GetIndexB();
    if (m_groupBuffer[a] != m_groupBuffer[b])
    {
      float32 w = contact.GetWeight();
      b2Vec2 n = contact.GetNormal();
      float32 h = m_depthBuffer[a] + m_depthBuffer[b];
      b2Vec2 f = ejectionStrength * h * w * n;
      m_velocityBuffer.data[a] -= f;
      m_velocityBuffer.data[b] += f;
    }
  }
}

void b2ParticleSystem::SolveForce(const b2TimeStep& step)
{
  float32 velocityPerForce = step.dt * GetParticleInvMass();
  for (int32 i = 0; i < m_count; i++)
  {
    m_velocityBuffer.data[i] += velocityPerForce * m_forceBuffer[i];
  }
  m_hasForce = false;
}

void b2ParticleSystem::SolveColorMixing()
{
  // mixes color between contacting particles
  b2Assert(m_colorBuffer.data);
  const int32 colorMixing128 = (int32) (128 * m_def.colorMixingStrength);
  if (colorMixing128) {
    for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
    {
      const b2ParticleContact& contact = m_contactBuffer[k];
      int32 a = contact.GetIndexA();
      int32 b = contact.GetIndexB();
      if (m_flagsBuffer.data[a] & m_flagsBuffer.data[b] &
        b2_colorMixingParticle)
      {
        b2ParticleColor& colorA = m_colorBuffer.data[a];
        b2ParticleColor& colorB = m_colorBuffer.data[b];
        // Use the static method to ensure certain compilers inline
        // this correctly.
        b2ParticleColor::MixColors(&colorA, &colorB, colorMixing128);
      }
    }
  }
}

void b2ParticleSystem::SolveZombie()
{
  // removes particles with zombie flag
  int32 newCount = 0;
  int32* newIndices = (int32*) m_world->m_stackAllocator.Allocate(
    sizeof(int32) * m_count);
  uint32 allParticleFlags = 0;
  for (int32 i = 0; i < m_count; i++)
  {
    int32 flags = m_flagsBuffer.data[i];
    if (flags & b2_zombieParticle)
    {
      b2DestructionListener * const destructionListener =
        m_world->m_destructionListener;
      if ((flags & b2_destructionListenerParticle) &&
        destructionListener)
      {
        destructionListener->SayGoodbye(this, i);
      }
      // Destroy particle handle.
      if (m_handleIndexBuffer.data)
      {
        b2ParticleHandle * const handle = m_handleIndexBuffer.data[i];
        if (handle)
        {
          handle->SetIndex(b2_invalidParticleIndex);
          m_handleIndexBuffer.data[i] = NULL;
          m_handleAllocator.Free(handle);
        }
      }
      newIndices[i] = b2_invalidParticleIndex;
    }
    else
    {
      newIndices[i] = newCount;
      if (i != newCount)
      {
        // Update handle to reference new particle index.
        if (m_handleIndexBuffer.data)
        {
          b2ParticleHandle * const handle =
            m_handleIndexBuffer.data[i];
          if (handle) handle->SetIndex(newCount);
          m_handleIndexBuffer.data[newCount] = handle;
        }
        m_flagsBuffer.data[newCount] = m_flagsBuffer.data[i];
        if (m_lastBodyContactStepBuffer.data)
        {
          m_lastBodyContactStepBuffer.data[newCount] =
            m_lastBodyContactStepBuffer.data[i];
        }
        if (m_bodyContactCountBuffer.data)
        {
          m_bodyContactCountBuffer.data[newCount] =
            m_bodyContactCountBuffer.data[i];
        }
        if (m_consecutiveContactStepsBuffer.data)
        {
          m_consecutiveContactStepsBuffer.data[newCount] =
            m_consecutiveContactStepsBuffer.data[i];
        }
        m_positionBuffer.data[newCount] = m_positionBuffer.data[i];
        m_velocityBuffer.data[newCount] = m_velocityBuffer.data[i];
        m_groupBuffer[newCount] = m_groupBuffer[i];
        if (m_hasForce)
        {
          m_forceBuffer[newCount] = m_forceBuffer[i];
        }
        if (m_staticPressureBuffer)
        {
          m_staticPressureBuffer[newCount] =
            m_staticPressureBuffer[i];
        }
        if (m_depthBuffer)
        {
          m_depthBuffer[newCount] = m_depthBuffer[i];
        }
        if (m_colorBuffer.data)
        {
          m_colorBuffer.data[newCount] = m_colorBuffer.data[i];
        }
        if (m_userDataBuffer.data)
        {
          m_userDataBuffer.data[newCount] = m_userDataBuffer.data[i];
        }
        if (m_expirationTimeBuffer.data)
        {
          m_expirationTimeBuffer.data[newCount] =
            m_expirationTimeBuffer.data[i];
        }
      }
      newCount++;
      allParticleFlags |= flags;
    }
  }

  // predicate functions
  struct Test
  {
    static bool IsProxyInvalid(const Proxy& proxy)
    {
      return proxy.index < 0;
    }
    static bool IsContactInvalid(const b2ParticleContact& contact)
    {
      return contact.GetIndexA() < 0 || contact.GetIndexB() < 0;
    }
    static bool IsBodyContactInvalid(const b2ParticleBodyContact& contact)
    {
      return contact.index < 0;
    }
    static bool IsPairInvalid(const b2ParticlePair& pair)
    {
      return pair.indexA < 0 || pair.indexB < 0;
    }
    static bool IsTriadInvalid(const b2ParticleTriad& triad)
    {
      return triad.indexA < 0 || triad.indexB < 0 || triad.indexC < 0;
    }
  };

  // update proxies
  for (int32 k = 0; k < m_proxyBuffer.GetCount(); k++)
  {
    Proxy& proxy = m_proxyBuffer.Begin()[k];
    proxy.index = newIndices[proxy.index];
  }
  m_proxyBuffer.RemoveIf(Test::IsProxyInvalid);

  // update contacts
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    b2ParticleContact& contact = m_contactBuffer[k];
    contact.SetIndices(newIndices[contact.GetIndexA()],
               newIndices[contact.GetIndexB()]);
  }
  m_contactBuffer.RemoveIf(Test::IsContactInvalid);

  // update particle-body contacts
  for (int32 k = 0; k < m_bodyContactBuffer.GetCount(); k++)
  {
    b2ParticleBodyContact& contact = m_bodyContactBuffer[k];
    contact.index = newIndices[contact.index];
  }
  m_bodyContactBuffer.RemoveIf(Test::IsBodyContactInvalid);

  // update pairs
  for (int32 k = 0; k < m_pairBuffer.GetCount(); k++)
  {
    b2ParticlePair& pair = m_pairBuffer[k];
    pair.indexA = newIndices[pair.indexA];
    pair.indexB = newIndices[pair.indexB];
  }
  m_pairBuffer.RemoveIf(Test::IsPairInvalid);

  // update triads
  for (int32 k = 0; k < m_triadBuffer.GetCount(); k++)
  {
    b2ParticleTriad& triad = m_triadBuffer[k];
    triad.indexA = newIndices[triad.indexA];
    triad.indexB = newIndices[triad.indexB];
    triad.indexC = newIndices[triad.indexC];
  }
  m_triadBuffer.RemoveIf(Test::IsTriadInvalid);

  // Update lifetime indices.
  if (m_indexByExpirationTimeBuffer.data)
  {
    int32 writeOffset = 0;
    for (int32 readOffset = 0; readOffset < m_count; readOffset++)
    {
      const int32 newIndex = newIndices[
        m_indexByExpirationTimeBuffer.data[readOffset]];
      if (newIndex != b2_invalidParticleIndex)
      {
        m_indexByExpirationTimeBuffer.data[writeOffset++] = newIndex;
      }
    }
  }

  // update groups
  for (b2ParticleGroup* group = m_groupList; group; group = group->GetNext())
  {
    int32 firstIndex = newCount;
    int32 lastIndex = 0;
    bool modified = false;
    for (int32 i = group->m_firstIndex; i < group->m_lastIndex; i++)
    {
      int32 j = newIndices[i];
      if (j >= 0) {
        firstIndex = b2Min(firstIndex, j);
        lastIndex = b2Max(lastIndex, j + 1);
      } else {
        modified = true;
      }
    }
    if (firstIndex < lastIndex)
    {
      group->m_firstIndex = firstIndex;
      group->m_lastIndex = lastIndex;
      if (modified)
      {
        if (group->m_groupFlags & b2_solidParticleGroup)
        {
          SetGroupFlags(group,
                  group->m_groupFlags |
                  b2_particleGroupNeedsUpdateDepth);
        }
      }
    }
    else
    {
      group->m_firstIndex = 0;
      group->m_lastIndex = 0;
      if (!(group->m_groupFlags & b2_particleGroupCanBeEmpty))
      {
        SetGroupFlags(group,
          group->m_groupFlags | b2_particleGroupWillBeDestroyed);
      }
    }
  }

  // update particle count
  m_count = newCount;
  m_world->m_stackAllocator.Free(newIndices);
  m_allParticleFlags = allParticleFlags;
  m_needsUpdateAllParticleFlags = false;

  // destroy bodies with no particles
  for (b2ParticleGroup* group = m_groupList; group;)
  {
    b2ParticleGroup* next = group->GetNext();
    if (group->m_groupFlags & b2_particleGroupWillBeDestroyed)
    {
      DestroyParticleGroup(group);
    }
    group = next;
  }
}

/// Destroy all particles which have outlived their lifetimes set by
/// SetParticleLifetime().
void b2ParticleSystem::SolveLifetimes(const b2TimeStep& step)
{
  b2Assert(m_expirationTimeBuffer.data);
  b2Assert(m_indexByExpirationTimeBuffer.data);
  // Update the time elapsed.
  m_timeElapsed = LifetimeToExpirationTime(step.dt);
  // Get the floor (non-fractional component) of the elapsed time.
  const int32 quantizedTimeElapsed = GetQuantizedTimeElapsed();

  const int32* const expirationTimes = m_expirationTimeBuffer.data;
  int32* const expirationTimeIndices = m_indexByExpirationTimeBuffer.data;
  const int32 particleCount = GetParticleCount();
  // Sort the lifetime buffer if it's required.
  if (m_expirationTimeBufferRequiresSorting)
  {
    const ExpirationTimeComparator expirationTimeComparator(
      expirationTimes);
    std::sort(expirationTimeIndices,
          expirationTimeIndices + particleCount,
          expirationTimeComparator);
    m_expirationTimeBufferRequiresSorting = false;
  }

  // Destroy particles which have expired.
  for (int32 i = particleCount - 1; i >= 0; --i)
  {
    const int32 particleIndex = expirationTimeIndices[i];
    const int32 expirationTime = expirationTimes[particleIndex];
    // If no particles need to be destroyed, skip this.
    if (quantizedTimeElapsed < expirationTime || expirationTime <= 0)
    {
      break;
    }
    // Destroy this particle.
    DestroyParticle(particleIndex);
  }
}

void b2ParticleSystem::RotateBuffer(int32 start, int32 mid, int32 end)
{
  // move the particles assigned to the given group toward the end of array
  if (start == mid || mid == end)
  {
    return;
  }
  b2Assert(mid >= start && mid <= end);
  struct NewIndices
  {
    int32 operator[](int32 i) const
    {
      if (i < start)
      {
        return i;
      }
      else if (i < mid)
      {
        return i + end - mid;
      }
      else if (i < end)
      {
        return i + start - mid;
      }
      else
      {
        return i;
      }
    }
    int32 start, mid, end;
  } newIndices;
  newIndices.start = start;
  newIndices.mid = mid;
  newIndices.end = end;

  std::rotate(m_flagsBuffer.data + start, m_flagsBuffer.data + mid,
        m_flagsBuffer.data + end);
  if (m_lastBodyContactStepBuffer.data)
  {
    std::rotate(m_lastBodyContactStepBuffer.data + start,
          m_lastBodyContactStepBuffer.data + mid,
          m_lastBodyContactStepBuffer.data + end);
  }
  if (m_bodyContactCountBuffer.data)
  {
    std::rotate(m_bodyContactCountBuffer.data + start,
          m_bodyContactCountBuffer.data + mid,
          m_bodyContactCountBuffer.data + end);
  }
  if (m_consecutiveContactStepsBuffer.data)
  {
    std::rotate(m_consecutiveContactStepsBuffer.data + start,
          m_consecutiveContactStepsBuffer.data + mid,
          m_consecutiveContactStepsBuffer.data + end);
  }
  std::rotate(m_positionBuffer.data + start, m_positionBuffer.data + mid,
        m_positionBuffer.data + end);
  std::rotate(m_velocityBuffer.data + start, m_velocityBuffer.data + mid,
        m_velocityBuffer.data + end);
  std::rotate(m_groupBuffer + start, m_groupBuffer + mid,
        m_groupBuffer + end);
  if (m_hasForce)
  {
    std::rotate(m_forceBuffer + start, m_forceBuffer + mid,
          m_forceBuffer + end);
  }
  if (m_staticPressureBuffer)
  {
    std::rotate(m_staticPressureBuffer + start,
          m_staticPressureBuffer + mid,
          m_staticPressureBuffer + end);
  }
  if (m_depthBuffer)
  {
    std::rotate(m_depthBuffer + start, m_depthBuffer + mid,
          m_depthBuffer + end);
  }
  if (m_colorBuffer.data)
  {
    std::rotate(m_colorBuffer.data + start,
          m_colorBuffer.data + mid, m_colorBuffer.data + end);
  }
  if (m_userDataBuffer.data)
  {
    std::rotate(m_userDataBuffer.data + start,
          m_userDataBuffer.data + mid, m_userDataBuffer.data + end);
  }

  // Update handle indices.
  if (m_handleIndexBuffer.data)
  {
    std::rotate(m_handleIndexBuffer.data + start,
          m_handleIndexBuffer.data + mid,
          m_handleIndexBuffer.data + end);
    for (int32 i = start; i < end; ++i)
    {
      b2ParticleHandle * const handle = m_handleIndexBuffer.data[i];
      if (handle) handle->SetIndex(newIndices[handle->GetIndex()]);
    }
  }

  if (m_expirationTimeBuffer.data)
  {
    std::rotate(m_expirationTimeBuffer.data + start,
          m_expirationTimeBuffer.data + mid,
          m_expirationTimeBuffer.data + end);
    // Update expiration time buffer indices.
    const int32 particleCount = GetParticleCount();
    int32* const indexByExpirationTime =
      m_indexByExpirationTimeBuffer.data;
    for (int32 i = 0; i < particleCount; ++i)
    {
      indexByExpirationTime[i] = newIndices[indexByExpirationTime[i]];
    }
  }

  // update proxies
  for (int32 k = 0; k < m_proxyBuffer.GetCount(); k++)
  {
    Proxy& proxy = m_proxyBuffer.Begin()[k];
    proxy.index = newIndices[proxy.index];
  }

  // update contacts
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    b2ParticleContact& contact = m_contactBuffer[k];
    contact.SetIndices(newIndices[contact.GetIndexA()],
               newIndices[contact.GetIndexB()]);
  }

  // update particle-body contacts
  for (int32 k = 0; k < m_bodyContactBuffer.GetCount(); k++)
  {
    b2ParticleBodyContact& contact = m_bodyContactBuffer[k];
    contact.index = newIndices[contact.index];
  }

  // update pairs
  for (int32 k = 0; k < m_pairBuffer.GetCount(); k++)
  {
    b2ParticlePair& pair = m_pairBuffer[k];
    pair.indexA = newIndices[pair.indexA];
    pair.indexB = newIndices[pair.indexB];
  }

  // update triads
  for (int32 k = 0; k < m_triadBuffer.GetCount(); k++)
  {
    b2ParticleTriad& triad = m_triadBuffer[k];
    triad.indexA = newIndices[triad.indexA];
    triad.indexB = newIndices[triad.indexB];
    triad.indexC = newIndices[triad.indexC];
  }

  // update groups
  for (b2ParticleGroup* group = m_groupList; group; group = group->GetNext())
  {
    group->m_firstIndex = newIndices[group->m_firstIndex];
    group->m_lastIndex = newIndices[group->m_lastIndex - 1] + 1;
  }
}

/// Set the lifetime (in seconds) of a particle relative to the current
/// time.
void b2ParticleSystem::SetParticleLifetime(const int32 index,
                       const float32 lifetime)
{
  b2Assert(ValidateParticleIndex(index));
  const bool initializeExpirationTimes =
    m_indexByExpirationTimeBuffer.data == NULL;
  m_expirationTimeBuffer.data = RequestBuffer(
    m_expirationTimeBuffer.data);
  m_indexByExpirationTimeBuffer.data = RequestBuffer(
    m_indexByExpirationTimeBuffer.data);

  // Initialize the inverse mapping buffer.
  if (initializeExpirationTimes)
  {
    const int32 particleCount = GetParticleCount();
    for (int32 i = 0; i < particleCount; ++i)
    {
      m_indexByExpirationTimeBuffer.data[i] = i;
    }
  }
  const int32 quantizedLifetime = (int32)(lifetime /
                      m_def.lifetimeGranularity);
  // Use a negative lifetime so that it's possible to track which
  // of the infinite lifetime particles are older.
  const int32 newExpirationTime = quantizedLifetime > 0 ?
    GetQuantizedTimeElapsed() + quantizedLifetime : quantizedLifetime;
  if (newExpirationTime != m_expirationTimeBuffer.data[index])
  {
    m_expirationTimeBuffer.data[index] = newExpirationTime;
    m_expirationTimeBufferRequiresSorting = true;
  }
}


/// Convert a lifetime value in returned by GetExpirationTimeBuffer()
/// to a value in seconds relative to the current simulation time.
float32 b2ParticleSystem::ExpirationTimeToLifetime(
  const int32 expirationTime) const
{
  return (float32)(expirationTime > 0 ?
            expirationTime - GetQuantizedTimeElapsed() :
            expirationTime) * m_def.lifetimeGranularity;
}

/// Get the lifetime (in seconds) of a particle relative to the current
/// time.
float32 b2ParticleSystem::GetParticleLifetime(const int32 index)
{
  b2Assert(ValidateParticleIndex(index));
  return ExpirationTimeToLifetime(GetExpirationTimeBuffer()[index]);
}

/// Get the array of particle lifetimes indexed by particle index.
/// GetParticleCount() items are in the returned array.
const int32* b2ParticleSystem::GetExpirationTimeBuffer()
{
  m_expirationTimeBuffer.data = RequestBuffer(
    m_expirationTimeBuffer.data);
  return m_expirationTimeBuffer.data;
}

/// Get the array of particle indices ordered by lifetime.
/// GetExpirationTimeBuffer(
///    GetIndexByExpirationTimeBuffer()[index])
/// is equivalent to GetParticleLifetime(index).
/// GetParticleCount() items are in the returned array.
const int32* b2ParticleSystem::GetIndexByExpirationTimeBuffer()
{
  // If particles are present, initialize / reinitialize the lifetime buffer.
  if (GetParticleCount())
  {
    SetParticleLifetime(0, GetParticleLifetime(0));
  }
  else
  {
    m_indexByExpirationTimeBuffer.data = RequestBuffer(
      m_indexByExpirationTimeBuffer.data);
  }
  return m_indexByExpirationTimeBuffer.data;
}

void b2ParticleSystem::SetDestructionByAge(const bool enable)
{
  if (enable)
  {
    GetExpirationTimeBuffer();
  }
  m_def.destroyByAge = enable;
}

/// Get the time elapsed in b2ParticleSystemDef::lifetimeGranularity.
int32 b2ParticleSystem::GetQuantizedTimeElapsed() const
{
  return (int32)(m_timeElapsed >> 32);
}

/// Convert a lifetime in seconds to an expiration time.
int64 b2ParticleSystem::LifetimeToExpirationTime(const float32 lifetime) const
{
  return m_timeElapsed + (int64)((lifetime / m_def.lifetimeGranularity) *
                   (float32)(1LL << 32));
}

template <typename T> void b2ParticleSystem::SetUserOverridableBuffer(
  UserOverridableBuffer<T>* buffer, T* newData, int32 newCapacity)
{
  b2Assert((newData && newCapacity) || (!newData && !newCapacity));
  if (!buffer->userSuppliedCapacity && buffer->data)
  {
    m_world->m_blockAllocator.Free(
      buffer->data, sizeof(T) * m_internalAllocatedCapacity);
  }
  buffer->data = newData;
  buffer->userSuppliedCapacity = newCapacity;
}

void b2ParticleSystem::SetFlagsBuffer(uint32* buffer, int32 capacity)
{
  SetUserOverridableBuffer(&m_flagsBuffer, buffer, capacity);
}

void b2ParticleSystem::SetPositionBuffer(b2Vec2* buffer,
                         int32 capacity)
{
  SetUserOverridableBuffer(&m_positionBuffer, buffer, capacity);
}

void b2ParticleSystem::SetVelocityBuffer(b2Vec2* buffer,
                         int32 capacity)
{
  SetUserOverridableBuffer(&m_velocityBuffer, buffer, capacity);
}

void b2ParticleSystem::SetColorBuffer(b2ParticleColor* buffer,
                        int32 capacity)
{
  SetUserOverridableBuffer(&m_colorBuffer, buffer, capacity);
}

void b2ParticleSystem::SetUserDataBuffer(void** buffer, int32 capacity)
{
  SetUserOverridableBuffer(&m_userDataBuffer, buffer, capacity);
}

void b2ParticleSystem::SetParticleFlags(int32 index, uint32 newFlags)
{
  uint32* oldFlags = &m_flagsBuffer.data[index];
  if (*oldFlags & ~newFlags)
  {
    // If any flags might be removed
    m_needsUpdateAllParticleFlags = true;
  }
  if (~m_allParticleFlags & newFlags)
  {
    // If any flags were added
    if (newFlags & b2_tensileParticle)
    {
      m_accumulation2Buffer = RequestBuffer(
        m_accumulation2Buffer);
    }
    if (newFlags & b2_colorMixingParticle)
    {
      m_colorBuffer.data = RequestBuffer(m_colorBuffer.data);
    }
    m_allParticleFlags |= newFlags;
  }
  *oldFlags = newFlags;
}

void b2ParticleSystem::SetGroupFlags(
  b2ParticleGroup* group, uint32 newFlags)
{
  uint32* oldFlags = &group->m_groupFlags;
  if ((*oldFlags ^ newFlags) & b2_solidParticleGroup)
  {
    // If the b2_solidParticleGroup flag changed schedule depth update.
    newFlags |= b2_particleGroupNeedsUpdateDepth;
  }
  if (*oldFlags & ~newFlags)
  {
    // If any flags might be removed
    m_needsUpdateAllGroupFlags = true;
  }
  if (~m_allGroupFlags & newFlags)
  {
    // If any flags were added
    if (newFlags & b2_solidParticleGroup)
    {
      m_depthBuffer = RequestBuffer(m_depthBuffer);
    }
    m_allGroupFlags |= newFlags;
  }
  *oldFlags = newFlags;
}

static inline bool IsSignificantForce(const b2Vec2& force)
{
  return force.x != 0 || force.y != 0;
}

inline bool b2ParticleSystem::ForceCanBeApplied(uint32 flags) const
{
  return !(flags & b2_wallParticle);
}

inline void b2ParticleSystem::PrepareForceBuffer()
{
  if (!m_hasForce)
  {
    memset(m_forceBuffer, 0, sizeof(*m_forceBuffer) * m_count);
    m_hasForce = true;
  }
}

void b2ParticleSystem::ApplyForce(int32 firstIndex, int32 lastIndex,
                  const b2Vec2& force)
{
  // Ensure we're not trying to apply force to particles that can't move,
  // such as wall particles.
#if B2_ASSERT_ENABLED
  uint32 flags = 0;
  for (int32 i = firstIndex; i < lastIndex; i++)
  {
    flags |= m_flagsBuffer.data[i];
  }
  b2Assert(ForceCanBeApplied(flags));
#endif

  // Early out if force does nothing (optimization).
  const b2Vec2 distributedForce = force / (float32)(lastIndex - firstIndex);
  if (IsSignificantForce(distributedForce))
  {
    PrepareForceBuffer();

    // Distribute the force over all the particles.
    for (int32 i = firstIndex; i < lastIndex; i++)
    {
      m_forceBuffer[i] += distributedForce;
    }
  }
}

void b2ParticleSystem::ParticleApplyForce(int32 index, const b2Vec2& force)
{
  if (IsSignificantForce(force) &&
    ForceCanBeApplied(m_flagsBuffer.data[index]))
  {
    PrepareForceBuffer();
    m_forceBuffer[index] += force;
  }
}

void b2ParticleSystem::ApplyLinearImpulse(int32 firstIndex, int32 lastIndex,
                      const b2Vec2& impulse)
{
  const float32 numParticles = (float32)(lastIndex - firstIndex);
  const float32 totalMass = numParticles * GetParticleMass();
  const b2Vec2 velocityDelta = impulse / totalMass;
  for (int32 i = firstIndex; i < lastIndex; i++)
  {
    m_velocityBuffer.data[i] += velocityDelta;
  }
}

void b2ParticleSystem::QueryAABB(b2QueryCallback* callback,
                 const b2AABB& aabb) const
{
  if (m_proxyBuffer.GetCount() == 0)
  {
    return;
  }
  const Proxy* beginProxy = m_proxyBuffer.Begin();
  const Proxy* endProxy = m_proxyBuffer.End();
  const Proxy* firstProxy = std::lower_bound(
    beginProxy, endProxy,
    computeTag(
      m_inverseDiameter * aabb.lowerBound.x,
      m_inverseDiameter * aabb.lowerBound.y));
  const Proxy* lastProxy = std::upper_bound(
    firstProxy, endProxy,
    computeTag(
      m_inverseDiameter * aabb.upperBound.x,
      m_inverseDiameter * aabb.upperBound.y));
  for (const Proxy* proxy = firstProxy; proxy < lastProxy; ++proxy)
  {
    int32 i = proxy->index;
    const b2Vec2& p = m_positionBuffer.data[i];
    if (aabb.lowerBound.x < p.x && p.x < aabb.upperBound.x &&
      aabb.lowerBound.y < p.y && p.y < aabb.upperBound.y)
    {
      if (!callback->ReportParticle(this, i))
      {
        break;
      }
    }
  }
}

void b2ParticleSystem::QueryShapeAABB(b2QueryCallback* callback,
                    const b2Shape& shape,
                    const b2Transform& xf) const
{
  b2AABB aabb;
  shape.ComputeAABB(&aabb, xf);
  QueryAABB(callback, aabb);
}

void b2ParticleSystem::RayCast(b2RayCastCallback* callback,
                 const b2Vec2& point1,
                 const b2Vec2& point2) const
{
  if (m_proxyBuffer.GetCount() == 0)
  {
    return;
  }
  b2AABB aabb;
  aabb.lowerBound = b2Min(point1, point2);
  aabb.upperBound = b2Max(point1, point2);
  float32 fraction = 1;
  // solving the following equation:
  // ((1-t)*point1+t*point2-position)^2=diameter^2
  // where t is a potential fraction
  b2Vec2 v = point2 - point1;
  float32 v2 = b2Dot(v, v);
  InsideBoundsEnumerator enumerator = GetInsideBoundsEnumerator(aabb);
  int32 i;
  while ((i = enumerator.GetNext()) >= 0)
  {
    b2Vec2 p = point1 - m_positionBuffer.data[i];
    float32 pv = b2Dot(p, v);
    float32 p2 = b2Dot(p, p);
    float32 determinant = pv * pv - v2 * (p2 - m_squaredDiameter);
    if (determinant >= 0)
    {
      float32 sqrtDeterminant = b2Sqrt(determinant);
      // find a solution between 0 and fraction
      float32 t = (-pv - sqrtDeterminant) / v2;
      if (t > fraction)
      {
        continue;
      }
      if (t < 0)
      {
        t = (-pv + sqrtDeterminant) / v2;
        if (t < 0 || t > fraction)
        {
          continue;
        }
      }
      b2Vec2 n = p + t * v;
      n.Normalize();
      float32 f = callback->ReportParticle(this, i, point1 + t * v, n, t);
      fraction = b2Min(fraction, f);
      if (fraction <= 0)
      {
        break;
      }
    }
  }
}

float32 b2ParticleSystem::ComputeCollisionEnergy() const
{
  float32 sum_v2 = 0;
  for (int32 k = 0; k < m_contactBuffer.GetCount(); k++)
  {
    const b2ParticleContact& contact = m_contactBuffer[k];
    int32 a = contact.GetIndexA();
    int32 b = contact.GetIndexB();
    b2Vec2 n = contact.GetNormal();
    b2Vec2 v = m_velocityBuffer.data[b] - m_velocityBuffer.data[a];
    float32 vn = b2Dot(v, n);
    if (vn < 0)
    {
      sum_v2 += vn * vn;
    }
  }
  return 0.5f * GetParticleMass() * sum_v2;
}

void b2ParticleSystem::SetStuckThreshold(int32 steps)
{
  m_stuckThreshold = steps;

  if (steps > 0)
  {
    m_lastBodyContactStepBuffer.data = RequestBuffer(
      m_lastBodyContactStepBuffer.data);
    m_bodyContactCountBuffer.data = RequestBuffer(
      m_bodyContactCountBuffer.data);
    m_consecutiveContactStepsBuffer.data = RequestBuffer(
      m_consecutiveContactStepsBuffer.data);
  }
}

#if LIQUIDFUN_EXTERNAL_LANGUAGE_API

b2ParticleSystem::b2ExceptionType b2ParticleSystem::IsBufCopyValid(
  int startIndex, int numParticles, int copySize, int bufSize) const
{
  const int maxNumParticles = GetParticleCount();

  // are we actually copying?
  if (copySize == 0)
  {
    return b2_noExceptions;
  }

  // is the index out of bounds?
  if (startIndex < 0 ||
    startIndex >= maxNumParticles ||
    numParticles < 0 ||
    numParticles + startIndex > maxNumParticles)
  {
    return b2_particleIndexOutOfBounds;
  }

  // are we copying within the boundaries?
  if (copySize > bufSize)
  {
    return b2_bufferTooSmall;
  }

  return b2_noExceptions;
}

#endif // LIQUIDFUN_EXTERNAL_LANGUAGE_API

#endif // ENABLE_LIQUID
